search

robotology-legacy / codyco-modules

Whole-body Compliant Dynamical Contacts in Cognitive Humanoids
www.codyco.eu
Other
19 stars 13 forks source link

[wholeBodyDynamicsTree] Running WBDT on gazebo model of iCubHeidelberg #190

Closed hu-yue closed 8 years ago

hu-yue commented 8 years ago

Hi, I'm having some problems running WBDT on our iCubHeidelberg model, it ends with these errors:

[FATAL]Assertion failure at /home/yue/iCubSW/codyco-superbuild/main/codyco-modules/src/modules/wholeBodyDynamicsTree/src/wholeBodyDynamicsStatesInterfaces.cpp:439 (robot_estimation_model->getSkinDynLibAlias(iDynTree_default_contact_link,iDynTree_default_contact_link_skinFrame,skinDynLib_body_part,skinDynLib_link_index))
/home/yue/iCubSW/yarp/build/lib/libYARP_OS.so.1(_ZNK4yarp2os3Log5fatalEPKcz+0x160) [0x7f6f07cd8d80]
wholeBodyDynamicsTree(_ZN35ExternalWrenchesAndTorquesEstimator17getDefaultContactEi+0xcb) [0x43fd4b]
wholeBodyDynamicsTree(_ZN35ExternalWrenchesAndTorquesEstimator16readSkinContactsEv+0x60e) [0x44045e]
wholeBodyDynamicsTree(_ZN35ExternalWrenchesAndTorquesEstimator39estimateExternalWrenchAndInternalJointsER16RobotJointStatusR17RobotSensorStatus+0x2f2) [0x4417a2]
wholeBodyDynamicsTree(_ZN23wholeBodyDynamicsThread14estimation_runEv+0x36) [0x428646]
wholeBodyDynamicsTree(_ZN23wholeBodyDynamicsThread3runEv+0x151) [0x428901]
/home/yue/iCubSW/yarp/build/lib/libYARP_OS.so.1(_ZN25RateThreadCallbackAdapter3runEv+0xad) [0x7f6f07d589ed]
/home/yue/iCubSW/yarp/build/lib/libYARP_OS.so.1(_Z18theExecutiveBranchPv+0x75) [0x7f6f07d9e865]
/usr/lib/libACE-6.0.3.so(_ZN21ACE_OS_Thread_Adapter6invokeEv+0x86) [0x7f6f053b6b16]
/lib/x86_64-linux-gnu/libpthread.so.0(+0x8182) [0x7f6f04eb3182]
/lib/x86_64-linux-gnu/libc.so.6(clone+0x6d) [0x7f6f0573e4ad]

I'm using the model and configuration files as in here.

I don't have errors running WBDT on the full icub model, but I can't figure out the problem of our model. Also, I saw this sentence in the outputs:

The root link root_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.

Should I worry about this?

Full output of WBDT:

||| clearing context
||| adding context [wholeBodyDynamicsTree]
||| configuring
||| default config file specified as wholeBodyDynamicsTree.ini
||| checking [/home/yue/Documents/iCubHeidelberg01/iCubHeidelberg01_no_weights/conf/wholeBodyDynamicsTree.ini] (pwd)
||| found /home/yue/Documents/iCubHeidelberg01/iCubHeidelberg01_no_weights/conf/wholeBodyDynamicsTree.ini
yarp: Port /wholeBodyDynamicsTree/rpc:i active at tcp://129.206.197.44:10024
||| finding file [wbi_conf_file]
||| checking [/home/yue/Documents/iCubHeidelberg01/iCubHeidelberg01_no_weights/conf/yarpWholeBodyInterface.ini] (pwd)
||| found /home/yue/Documents/iCubHeidelberg01/iCubHeidelberg01_no_weights/conf/yarpWholeBodyInterface.ini
[INFO]RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /wholeBodyDynamicsTree/torso/rpc:o active at tcp://129.206.197.44:10025
yarp: Port /wholeBodyDynamicsTree/torso/command:o active at tcp://129.206.197.44:10026
yarp: Port /wholeBodyDynamicsTree/torso/state:i active at tcp://129.206.197.44:10027
yarp: Port /wholeBodyDynamicsTree/torso/stateExt:i active at tcp://129.206.197.44:10028
yarp: Sending output from /wholeBodyDynamicsTree/torso/rpc:o to /icubGazeboSim/torso/rpc:i using tcp
yarp: Sending output from /wholeBodyDynamicsTree/torso/command:o to /icubGazeboSim/torso/command:i using udp
yarp: Receiving input from /icubGazeboSim/torso/state:o to /wholeBodyDynamicsTree/torso/state:i using udp
yarp: Receiving input from /icubGazeboSim/torso/stateExt:o to /wholeBodyDynamicsTree/torso/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /wholeBodyDynamicsTree/left_leg/rpc:o active at tcp://129.206.197.44:10029
yarp: Port /wholeBodyDynamicsTree/left_leg/command:o active at tcp://129.206.197.44:10030
yarp: Port /wholeBodyDynamicsTree/left_leg/state:i active at tcp://129.206.197.44:10031
yarp: Port /wholeBodyDynamicsTree/left_leg/stateExt:i active at tcp://129.206.197.44:10032
yarp: Sending output from /wholeBodyDynamicsTree/left_leg/rpc:o to /icubGazeboSim/left_leg/rpc:i using tcp
yarp: Sending output from /wholeBodyDynamicsTree/left_leg/command:o to /icubGazeboSim/left_leg/command:i using udp
yarp: Receiving input from /icubGazeboSim/left_leg/state:o to /wholeBodyDynamicsTree/left_leg/state:i using udp
yarp: Receiving input from /icubGazeboSim/left_leg/stateExt:o to /wholeBodyDynamicsTree/left_leg/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /wholeBodyDynamicsTree/right_leg/rpc:o active at tcp://129.206.197.44:10033
yarp: Port /wholeBodyDynamicsTree/right_leg/command:o active at tcp://129.206.197.44:10034
yarp: Port /wholeBodyDynamicsTree/right_leg/state:i active at tcp://129.206.197.44:10035
yarp: Port /wholeBodyDynamicsTree/right_leg/stateExt:i active at tcp://129.206.197.44:10036
yarp: Sending output from /wholeBodyDynamicsTree/right_leg/rpc:o to /icubGazeboSim/right_leg/rpc:i using tcp
yarp: Sending output from /wholeBodyDynamicsTree/right_leg/command:o to /icubGazeboSim/right_leg/command:i using udp
yarp: Receiving input from /icubGazeboSim/right_leg/state:o to /wholeBodyDynamicsTree/right_leg/state:i using udp
yarp: Receiving input from /icubGazeboSim/right_leg/stateExt:o to /wholeBodyDynamicsTree/right_leg/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
yarp: Port /wholeBodyDynamicsTree/ftSens/l_leg_ft_sensor:i active at tcp://129.206.197.44:10037
yarp: Receiving input from /icubGazeboSim/left_leg/analog:o to /wholeBodyDynamicsTree/ftSens/l_leg_ft_sensor:i using udp
yarp: Port /wholeBodyDynamicsTree/ftSens/l_foot_ft_sensor:i active at tcp://129.206.197.44:10038
yarp: Receiving input from /icubGazeboSim/left_foot/analog:o to /wholeBodyDynamicsTree/ftSens/l_foot_ft_sensor:i using udp
yarp: Port /wholeBodyDynamicsTree/ftSens/r_leg_ft_sensor:i active at tcp://129.206.197.44:10039
yarp: Receiving input from /icubGazeboSim/right_leg/analog:o to /wholeBodyDynamicsTree/ftSens/r_leg_ft_sensor:i using udp
yarp: Port /wholeBodyDynamicsTree/ftSens/r_foot_ft_sensor:i active at tcp://129.206.197.44:10040
yarp: Receiving input from /icubGazeboSim/right_foot/analog:o to /wholeBodyDynamicsTree/ftSens/r_foot_ft_sensor:i using udp
yarp: Port /wholeBodyDynamicsTree/imu/imu_frame:i active at tcp://129.206.197.44:10041
yarp: Receiving input from /icubGazeboSim/inertial to /wholeBodyDynamicsTree/imu/imu_frame:i using udp
[INFO]Launching wholeBodyDynamicsThread with name :  wholeBodyDynamicsTree  and robotName  icubGazeboSim  and period  10 
||| finding file ../iCubHeidelberg01_no_weights.urdf
||| checking [/home/yue/Documents/iCubHeidelberg01/iCubHeidelberg01_no_weights/conf/../iCubHeidelberg01_no_weights.urdf] (pwd)
||| found /home/yue/Documents/iCubHeidelberg01/iCubHeidelberg01_no_weights/conf/../iCubHeidelberg01_no_weights.urdf
The root link root_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[INFO] TorqueEstimationTree constructor: loaded urdf with 15dofs and 4 fts ( 4) 
[DEBUG] TorqueEstimationTree: Setting id of dof torso_pitch to 0
[DEBUG] TorqueEstimationTree: Setting id of dof torso_roll to 1
[DEBUG] TorqueEstimationTree: Setting id of dof torso_yaw to 2
[DEBUG] TorqueEstimationTree: Setting id of dof l_hip_pitch to 3
[DEBUG] TorqueEstimationTree: Setting id of dof l_hip_roll to 4
[DEBUG] TorqueEstimationTree: Setting id of dof l_hip_yaw to 5
[DEBUG] TorqueEstimationTree: Setting id of dof l_knee to 6
[DEBUG] TorqueEstimationTree: Setting id of dof l_ankle_pitch to 7
[DEBUG] TorqueEstimationTree: Setting id of dof l_ankle_roll to 8
[DEBUG] TorqueEstimationTree: Setting id of dof r_hip_pitch to 9
[DEBUG] TorqueEstimationTree: Setting id of dof r_hip_roll to 10
[DEBUG] TorqueEstimationTree: Setting id of dof r_hip_yaw to 11
[DEBUG] TorqueEstimationTree: Setting id of dof r_knee to 12
[DEBUG] TorqueEstimationTree: Setting id of dof r_ankle_pitch to 13
[DEBUG] TorqueEstimationTree: Setting id of dof r_ankle_roll to 14
[INFO] TorqueEstimationTree constructor: loaded urdf with 15dofs and 4 fts ( 4) 
The root link root_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[INFO] TorqueEstimationTree constructor: loaded urdf with 15dofs and 4 fts ( 4) 
[DEBUG] TorqueEstimationTree: Setting id of dof torso_pitch to 0
[DEBUG] TorqueEstimationTree: Setting id of dof torso_roll to 1
[DEBUG] TorqueEstimationTree: Setting id of dof torso_yaw to 2
[DEBUG] TorqueEstimationTree: Setting id of dof l_hip_pitch to 3
[DEBUG] TorqueEstimationTree: Setting id of dof l_hip_roll to 4
[DEBUG] TorqueEstimationTree: Setting id of dof l_hip_yaw to 5
[DEBUG] TorqueEstimationTree: Setting id of dof l_knee to 6
[DEBUG] TorqueEstimationTree: Setting id of dof l_ankle_pitch to 7
[DEBUG] TorqueEstimationTree: Setting id of dof l_ankle_roll to 8
[DEBUG] TorqueEstimationTree: Setting id of dof r_hip_pitch to 9
[DEBUG] TorqueEstimationTree: Setting id of dof r_hip_roll to 10
[DEBUG] TorqueEstimationTree: Setting id of dof r_hip_yaw to 11
[DEBUG] TorqueEstimationTree: Setting id of dof r_knee to 12
[DEBUG] TorqueEstimationTree: Setting id of dof r_ankle_pitch to 13
[DEBUG] TorqueEstimationTree: Setting id of dof r_ankle_roll to 14
[INFO] TorqueEstimationTree constructor: loaded urdf with 15dofs and 4 fts ( 4) 
yarp: Port /wholeBodyDynamicsTree/skin_contacts:i active at tcp://129.206.197.44:10060
yarp: Port /wholeBodyDynamicsTree/contacts:o active at tcp://129.206.197.44:10061
[DEBUG] Create TorqueEstimationTree with 4 ft sensors
The root link root_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[INFO] TorqueEstimationTree constructor: loaded urdf with 15dofs and 4 fts ( 4) 
[DEBUG] TorqueEstimationTree: Setting id of dof torso_pitch to 0
[DEBUG] TorqueEstimationTree: Setting id of dof torso_roll to 1
[DEBUG] TorqueEstimationTree: Setting id of dof torso_yaw to 2
[DEBUG] TorqueEstimationTree: Setting id of dof l_hip_pitch to 3
[DEBUG] TorqueEstimationTree: Setting id of dof l_hip_roll to 4
[DEBUG] TorqueEstimationTree: Setting id of dof l_hip_yaw to 5
[DEBUG] TorqueEstimationTree: Setting id of dof l_knee to 6
[DEBUG] TorqueEstimationTree: Setting id of dof l_ankle_pitch to 7
[DEBUG] TorqueEstimationTree: Setting id of dof l_ankle_roll to 8
[DEBUG] TorqueEstimationTree: Setting id of dof r_hip_pitch to 9
[DEBUG] TorqueEstimationTree: Setting id of dof r_hip_roll to 10
[DEBUG] TorqueEstimationTree: Setting id of dof r_hip_yaw to 11
[DEBUG] TorqueEstimationTree: Setting id of dof r_knee to 12
[DEBUG] TorqueEstimationTree: Setting id of dof r_ankle_pitch to 13
[DEBUG] TorqueEstimationTree: Setting id of dof r_ankle_roll to 14
[INFO] TorqueEstimationTree constructor: loaded urdf with 15dofs and 4 fts ( 4) 

[INFO] IDYNTREE_SKINDYNLIB_LINKS correctly loaded
[DEBUG] robot_estimation_model->getSubTreeInternalDynamics().size() : 5
[DEBUG] torque_estimation_subtrees.size(): 5
[INFO] WBD_SUBTREES correctly loaded with 5subtrees
[DEBUG]ExternalWrenchesAndTorquesEstimator::init() terminated successfully 
[INFO]ControlBoards information correctly loaded from configuration file 
[INFO]calibration_support_link is  root_link 
yarp: Port /wholeBodyDynamicsTree/left_leg/cartesianEndEffectorWrench:o active at tcp://129.206.197.44:10062
yarp: Port /wholeBodyDynamicsTree/right_leg/cartesianEndEffectorWrench:o active at tcp://129.206.197.44:10063
yarp: Port /wholeBodyDynamicsTree/right_leg/endEffectorWrench:o active at tcp://129.206.197.44:10064
yarp: Port /wholeBodyDynamicsTree/left_leg/endEffectorWrench:o active at tcp://129.206.197.44:10065
yarp: Port /wholeBodyDynamicsTree/base:o active at tcp://129.206.197.44:10066
yarp: Port /wholeBodyDynamicsTree/filtered/inertial:o active at tcp://129.206.197.44:10067
[INFO]cutoff_ft option not found, disabling FT filtering 
[INFO]cutoff_velacc option not found, disabling joint velocities and accelerations filtering 
yarp: Port /wholeBodyDynamicsTree/left_leg/Torques:o active at tcp://129.206.197.44:10068
yarp: Port /wholeBodyDynamicsTree/torso/Torques:o active at tcp://129.206.197.44:10069
yarp: Port /wholeBodyDynamicsTree/right_leg/Torques:o active at tcp://129.206.197.44:10070
[INFO] SIMPLE_LEGGED_ODOMETRY group not found, odometry disabled 
[INFO] wholeBodyDynamicsThread::calibrateOffset called with code all
[INFO]wholeBodyDynamicsThread::calibrateOffset: current calibration for FT  0  is  0   0   0   0   0   0 
[INFO]wholeBodyDynamicsThread::calibrateOffset: current calibration for FT  1  is  0   0   0   0   0   0 
[INFO]wholeBodyDynamicsThread::calibrateOffset: current calibration for FT  2  is  0   0   0   0   0   0 
[INFO]wholeBodyDynamicsThread::calibrateOffset: current calibration for FT  3  is  0   0   0   0   0   0 
wholeBodyDynamicsThread::calibrateOffset all called successfully, starting calibration.
[INFO]wholeBodyDynamicsThread::threadInit finished successfully. 
[INFO]wholeBodyDynamicsThread started 
[INFO]wholeBodyDynamicsThread: complete calibration at system time :  02-06-2016 01:39:40 
[INFO]wholeBodyDynamicsThread: new calibration for FT  l_leg_ft_sensor  is  -0.299460    2.446531   -127.701017 -0.184446    0.057843    1.023892 
[INFO]wholeBodyDynamicsThread: new calibration for FT  l_foot_ft_sensor  is   0.075523   2.696045   -129.434631  0.433639    0.171676    1.048568 
[INFO]wholeBodyDynamicsThread: new calibration for FT  r_leg_ft_sensor  is  -0.326819   -2.923917   -132.348048  0.198533    0.096524   -1.036338 
[INFO]wholeBodyDynamicsThread: new calibration for FT  r_foot_ft_sensor  is  -0.048864  -2.715916   -128.949813 -0.502522    0.569644   -1.026135 
[FATAL]Assertion failure at /home/yue/iCubSW/codyco-superbuild/main/codyco-modules/src/modules/wholeBodyDynamicsTree/src/wholeBodyDynamicsStatesInterfaces.cpp:439 (robot_estimation_model->getSkinDynLibAlias(iDynTree_default_contact_link,iDynTree_default_contact_link_skinFrame,skinDynLib_body_part,skinDynLib_link_index))
/home/yue/iCubSW/yarp/build/lib/libYARP_OS.so.1(_ZNK4yarp2os3Log5fatalEPKcz+0x160) [0x7f6f07cd8d80]
wholeBodyDynamicsTree(_ZN35ExternalWrenchesAndTorquesEstimator17getDefaultContactEi+0xcb) [0x43fd4b]
wholeBodyDynamicsTree(_ZN35ExternalWrenchesAndTorquesEstimator16readSkinContactsEv+0x60e) [0x44045e]
wholeBodyDynamicsTree(_ZN35ExternalWrenchesAndTorquesEstimator39estimateExternalWrenchAndInternalJointsER16RobotJointStatusR17RobotSensorStatus+0x2f2) [0x4417a2]
wholeBodyDynamicsTree(_ZN23wholeBodyDynamicsThread14estimation_runEv+0x36) [0x428646]
wholeBodyDynamicsTree(_ZN23wholeBodyDynamicsThread3runEv+0x151) [0x428901]
/home/yue/iCubSW/yarp/build/lib/libYARP_OS.so.1(_ZN25RateThreadCallbackAdapter3runEv+0xad) [0x7f6f07d589ed]
/home/yue/iCubSW/yarp/build/lib/libYARP_OS.so.1(_Z18theExecutiveBranchPv+0x75) [0x7f6f07d9e865]
/usr/lib/libACE-6.0.3.so(_ZN21ACE_OS_Thread_Adapter6invokeEv+0x86) [0x7f6f053b6b16]
/lib/x86_64-linux-gnu/libpthread.so.0(+0x8182) [0x7f6f04eb3182]
/lib/x86_64-linux-gnu/libc.so.6(clone+0x6d) [0x7f6f0573e4ad]
traversaro commented 8 years ago

Sorry @hu-yue , I was outside of the lab for the weekend. Are you using YARP_ROBOT_NAME set to iCubHeidelberg01, right?

hu-yue commented 8 years ago

@traversaro yes it is iCubHeidelberg01, should it be something else?

hu-yue commented 8 years ago

@traversaro I solved the problem, I think the problem is that I was not using the correct urdf file. Anyway now it seems to work fine.