Hardware Fault on switch to Torque Mode

rlober commented 9 years ago

On iCubParis02 when I try to switch a joint into torque mode I get a HARDWARE FAULT error. Here is a snapshot of this: hardware fault in torque mode

Here is the terminal output (I tested on multiple joints):

[DEBUG]Opening interfaces...
joint: 0 in TORQUE mode!
joint: 0 in POSITION mode!
joint: 3 in TORQUE mode!
joint: 3 in POSITION mode!
joint: 3 in TORQUE mode!
joint: 3 in POSITION mode!
joint: 4 in TORQUE mode!
joint: 4 in POSITION mode!
joint: 3 in TORQUE mode!
joint: 3 in POSITION mode!

I don't get any error messages in yarplogger.

Any idea why this may be happening?

Ryan

traversaro commented 9 years ago

You probably don't have a working torque feedback. Have you launched the wholeBodyDynamics(Tree) with the --autoconnect option?

rlober commented 9 years ago

wholebodydyn

Is this what you are referring to? On little note: When I try to run wholeBodyDynamics on a cluster blade I get the following:

[ERR] (iCubStartup) cannot run wholeBodyDynamics on icub-b1 : cannot run wholeBodyDynamics on /icub-b1

but when I run it on icub-desktop it works.

rlober commented 9 years ago

@traversaro

Ok... Sorry, wholeBodyDynamics is not the same as wholeBodyDynamics(Tree). I am trying to get it to work right now, but I have deactivated the left leg so I have to reconfigure the .ini

What all should I comment out to make it ignore the left leg? Everything with "left_leg"?

update:

I tried changing just the yarpWholeBodyInterface.ini lines with left_leg ie: ROBOT_LEFT_LEG_JOINTS = () and I get the following assertion error:

[INFO]Launching wholeBodyDynamicsThread with name :  wholeBodyDynamicsTree  and robotName  icub  and period  10 
[INFO] TorqueEstimationTree constructor: loaded urdf with 26dofs and 4 fts ( 4) 
[FATAL]Assertion failure (dof_serialization.size() == serial.getNrOfDOFs())

rlober commented 9 years ago

@traversaro @francesco-romano

Just for my info, What is the difference between wholeBodyDynamics and wholeBodyDynamicsTree?

traversaro commented 9 years ago

They are totally different. wholeBodyDynamics is the legacy module that does not support floating-base estimation of the forces (it assume that the robot is fixed base) while wholeBodyDynamicsTree is the new module that compute the floating base whole body dynamics.

rlober commented 9 years ago

@traversaro

Thank you for the clarification on the two modules.

Can you help me with the following problem: I have deactivated the left leg because of CAN issues, so I have reconfigured the icub_all.xml with all of the left_leg/foot entries commented out.

What all should I comment out in wholeBodyDynamicsTree.ini to make it ignore the left leg?

I tried changing just the yarpWholeBodyInterface.ini lines with left_leg ie: ROBOT_LEFT_LEG_JOINTS = () and I get the following assertion error:

[INFO]Launching wholeBodyDynamicsThread with name :  wholeBodyDynamicsTree  and robotName  icub  and period  10 
[INFO] TorqueEstimationTree constructor: loaded urdf with 26dofs and 4 fts ( 4) 
[FATAL]Assertion failure (dof_serialization.size() == serial.getNrOfDOFs())

traversaro commented 9 years ago

Sorry, the configuration of the wholeBodyDynamicsTree is a bit under documented. To remove a part you should:

Remove the joints of that part from the ROBOT_DYNAMIC_MODEL_JOINTS in the yarpWholeBodyInterface.ini (this are the joint considered for the dynamical model used for the torque estimation)
Remove the joints of the part from the list of published torque ports in wholeBodyDynamicsTree.ini. This mean that in the [WBD_OUTPUT_TORQUE_PORTS] group, you can comment out the lines relative to the left_leg .

rlober commented 9 years ago

@traversaro

Great thanks man, I have some meetings this morning but I will give it a shot in the afternoon and let you know.

randaz81 commented 9 years ago

@traversaro Is a --no_legs flag available? (like in legacy WBD)

traversaro commented 9 years ago

@randaz81 not at the moment.. the problem is wholeBodyDynamicsTree is agnostic of the underlyng model, that could have any structure. Having an option to disable just some part is useful for sure, but I have to think about it for how to correct implement it.

rlober commented 9 years ago

Ok, so I tried commenting out the lines you mentioned @traversaro but I am still getting the same error. I have gone ahead and copied the relevent .ini's and the output:

icub_all.xml

<?xml version="1.0" encoding="UTF-8" ?>
<robot name="icub">

    <!-- cartesian -->
    <devices file="cartesian/left_arm_cartesian.xml" />
    <devices file="cartesian/right_arm_cartesian.xml" />

    <!-- motor controllers wrappers -->
    <devices file="wrappers/motorControl/left_arm_mc_wrapper.xml" />
    <devices file="wrappers/motorControl/right_arm_mc_wrapper.xml" />
    <!-- <devices file="wrappers/motorControl/left_leg_mc_wrapper.xml" />  -->
    <devices file="wrappers/motorControl/right_leg_mc_wrapper.xml" />
    <devices file="wrappers/motorControl/head_mc_wrapper.xml" />
    <devices file="wrappers/motorControl/torso_mc_wrapper.xml" />
    <devices file="hardware/motorControl/icub_left_arm.xml" />
    <devices file="hardware/motorControl/icub_left_hand.xml" />
    <devices file="hardware/motorControl/icub_right_arm.xml" />
    <devices file="hardware/motorControl/icub_right_hand.xml" />
    <!-- <devices file="hardware/motorControl/icub_left_leg.xml" />  -->
    <devices file="hardware/motorControl/icub_right_leg.xml" />
    <devices file="hardware/motorControl/icub_head.xml" />
    <devices file="hardware/motorControl/icub_torso.xml" />

    <!-- VIRTUAL ANALOG SERVERs -->
    <devices file="wrappers/VFT/left_arm_VFT_wrapper.xml" />
    <!-- <devices file="wrappers/VFT/left_leg_VFT_wrapper.xml" />  -->
    <devices file="wrappers/VFT/right_arm_VFT_wrapper.xml" />
    <devices file="wrappers/VFT/right_leg_VFT_wrapper.xml" />
    <devices file="wrappers/VFT/torso_VFT_wrapper.xml" />

    <devices file="hardware/VFT/left_arm_virtual_strain.xml" />
    <!-- <devices file="hardware/VFT/left_leg_virtual_strain.xml" />  -->
    <devices file="hardware/VFT/right_arm_virtual_strain.xml" />
    <devices file="hardware/VFT/right_leg_virtual_strain.xml" />
    <devices file="hardware/VFT/torso_virtual_strain.xml" />

    <!-- REAL ANALOG SENSORS -->
    <devices file="wrappers/FT/left_arm_FT_wrapper.xml" />
    <!-- <devices file="wrappers/FT/left_leg_FT_wrapper.xml" />  -->
    <devices file="wrappers/FT/right_arm_FT_wrapper.xml" />
    <devices file="wrappers/FT/right_leg_FT_wrapper.xml" />
    <devices file="wrappers/MAIS/left_hand_mais_wrapper.xml" />
    <devices file="wrappers/MAIS/right_hand_mais_wrapper.xml" />
    <devices file="hardware/FT/left_arm_strain.xml" />
    <!-- <devices file="hardware/FT/left_leg_strain.xml" />  -->
    <devices file="hardware/FT/right_arm_strain.xml" />
    <devices file="hardware/FT/right_leg_strain.xml" />
    <devices file="hardware/MAIS/left_hand_mais.xml" />
    <devices file="hardware/MAIS/right_hand_mais.xml" />

    <!-- <devices file="wrappers/FT/left_foot_FT_wrapper.xml" /> -->
    <devices file="wrappers/FT/right_foot_FT_wrapper.xml" />
    <!-- <devices file="hardware/FT/left_foot_strain.xml" />  -->
    <devices file="hardware/FT/right_foot_strain.xml" />

    <!-- 6SG SENSORS -->
<!--    <devices file="wrappers/6SG/left_arm_SG_wrapper.xml" />
    <devices file="wrappers/6SG/left_lower_leg_SG_wrapper.xml" />
    <devices file="wrappers/6SG/left_upper_leg_SG_wrapper.xml" />
    <devices file="wrappers/6SG/right_arm_SG_wrapper.xml" />
    <devices file="wrappers/6SG/right_lower_leg_SG_wrapper.xml" />
    <devices file="wrappers/6SG/right_upper_leg_SG_wrapper.xml" />
    <devices file="wrappers/6SG/torso_SG_wrapper.xml" />
    <devices file="hardware/6SG/left_arm_SG.xml" />
    <devices file="hardware/6SG/left_lower_leg_SG.xml" />
    <devices file="hardware/6SG/left_upper_leg_SG.xml" />
    <devices file="hardware/6SG/right_arm_SG.xml" />
    <devices file="hardware/6SG/right_lower_leg_SG.xml" />
    <devices file="hardware/6SG/right_upper_leg_SG.xml" />
    <devices file="hardware/6SG/torso_SG.xml" />
    -->
    <!-- SKIN -->
    <devices file="wrappers/skin/left_arm_skin_wrapper.xml" />
    <devices file="wrappers/skin/right_arm_skin_wrapper.xml" />
    <devices file="wrappers/skin/torso_skin_wrapper.xml" />
    <devices file="hardware/skin/left_arm.xml" />
    <devices file="hardware/skin/right_arm.xml" />
    <devices file="hardware/skin/torso.xml" />

    <!-- MTX INERTIAL SENSOR & SKIN INERTIAL SENSOR-->
    <devices file="hardware/inertial.xml" />
    <!--
    <devices file="wrappers/skin/left_hand_inertial_wrapper.xml" />
    <devices file="wrappers/skin/right_hand_inertial_wrapper.xml" />
    <devices file="hardware/skin/left_hand_inertial_mtb.xml" />
    <devices file="hardware/skin/right_hand_inertial_mtb.xml" />
    -->

    <!-- CALIBRATORS -->
    <devices file="calibrators/head_calib.xml" />
    <devices file="calibrators/torso_calib.xml" />
    <devices file="calibrators/right_leg_calib.xml" />
    <!-- <devices file="calibrators/left_leg_calib.xml" />  -->
    <devices file="calibrators/left_arm_calib.xml" />
    <devices file="calibrators/right_arm_calib.xml" />
    <devices file="calibrators/left_hand_calib.xml" />
    <devices file="calibrators/right_hand_calib.xml" />
</robot>

wholeBodyDynamicsTree.ini

name            wholeBodyDynamicsTree
period          10
robot           icub
cutoff          3.0

#This file is referenced here, but it
# should be found in a robot specific directory
wbi_conf_file yarpWholeBodyInterface.ini

# list of torque estimates that should be sent back
# to the robotInterface for closing the torque loop
# for each port, there is a list of joints and a magic
# number. If you want to understand more about this magic
# number, please ask to Marco Randazzo.
[WBD_OUTPUT_TORQUE_PORTS]
torso       = (4,(torso_yaw,torso_roll,torso_pitch))

#left_leg    = (2,(l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll))
left_leg    = (2,())

right_leg   = (2,(r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll))
left_arm    = (1,(l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_prosup))
right_arm   = (1,(r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_prosup))
head        = (0,(neck_pitch,neck_roll,neck_yaw))
left_wrist  = (3,(l_wrist_pitch,l_wrist_yaw))
right_wrist = (3,(r_wrist_pitch,r_wrist_yaw))

# list of port on which the estimate of the external wrench
# acting on a link is expressed. The element of this group
# have as a key the port name to be opened.
# Then two possible formats of subsequent bottle are possible:
#  * if the bottle has two elements, the first is the link name
#    for which the external wrench is streamed, and the second
#     one is the plucker frame in which the streamed wrench is expressed.
#  * if the bottle has three elements, the first is still the link,
#     while the point of expression of the wrench is the origin of the frame
#     whose name is the second in the bottle, and the orientation of the
#     wrench is the orientation of the frame passed third in the bottle
[WBD_OUTPUT_EXTERNAL_WRENCH_PORTS]
/${name}/left_arm/endEffectorWrench:o  = (l_hand,l_hand_dh_frame)
/${name}/right_arm/endEffectorWrench:o = (r_hand,r_hand_dh_frame)
/${name}/left_leg/endEffectorWrench:o  = (l_foot,l_foot_dh_frame)
/${name}/right_leg/endEffectorWrench:o = (r_foot,r_foot_dh_frame)
/${name}/left_arm/cartesianEndEffectorWrench:o  = (l_hand,l_hand_dh_frame,root_link)
/${name}/right_arm/cartesianEndEffectorWrench:o = (r_hand,r_hand_dh_frame,root_link)
/${name}/left_leg/cartesianEndEffectorWrench:o  = (l_foot,l_foot_dh_frame,root_link)
/${name}/right_leg/cartesianEndEffectorWrench:o = (r_foot,r_foot_dh_frame,root_link)

# map to wbi/urdf link names
# and skindynlib link (body_part,link_number)
# TORSO     : 1
# LEFT_ARM  : 3
# RIGHT_ARM : 4
# LEFT_LEG  : 5
# RIGHT_LEG : 6
#
[IDYNTREE_SKINDYNLIB_LINKS]
root_link = (root_link,1,0)
chest     = (chest_skin_frame,1,2)
l_upper_arm = (l_upper_arm_dh_frame,3,2)
l_forearm = (l_forearm_dh_frame,3,4)
l_hand    = (l_hand_dh_frame,3,6)
r_upper_arm     = (r_upper_arm_dh_frame,4,2)
r_forearm = (r_forearm_dh_frame,4,4)
r_hand    = (r_hand_dh_frame,4,6)
l_lower_leg     = (l_lower_leg,5,3)
l_ankle_1   = (l_ankle_1,5,4)
l_foot    = (l_foot_dh_frame,5,5)
r_lower_leg  = (r_lower_leg,6,3)
r_ankle_1   = (r_ankle_1,6,4)
r_foot    = (r_foot_dh_frame,6,5)

# Comment for WBD_SUBTREES
# for each subtree we specify the list of links belonging to that subtree,
# and the default contact link if no external contact information (i.e. skin)
# is provided
[WBD_SUBTREES]
torso_subtree       = ((root_link,l_hip_1,l_hip_2,r_hip_1,r_hip_2,torso_1,torso_2,chest,l_shoulder_1,l_shoulder_2,l_shoulder_3,r_shoulder_1,r_shoulder_2,r_shoulder_3),root_link)
left_leg_subtree    = ((l_hip_3,l_upper_leg,l_lower_leg,l_ankle_1,l_ankle_2),l_lower_leg)
right_leg_subtree   = ((r_hip_3,r_upper_leg,r_lower_leg,r_ankle_1,r_ankle_2),r_lower_leg)
left_foot_subtree   = ((l_foot),l_foot)
right_foot_subtree  = ((r_foot),r_foot)
left_arm_subtree   = ((l_upper_arm,l_elbow_1,l_forearm,l_wrist_1,l_hand),l_hand)
right_arm_subtree   = ((r_upper_arm,r_elbow_1,r_forearm,r_wrist_1,r_hand),r_hand)

# Options for simple legged odometry
[SIMPLE_LEGGED_ODOMETRY]
initial_world_frame l_sole
initial_fixed_link  l_foot
floating_base_frame root_link

yarpWholeBodyInterface.ini

robot icub
urdf model.urdf
getLimitsFromControlBoard
readSpeedAccFromControlBoard

#Original information extracted from http://eris.liralab.it/wiki/ICub_joints
#Every "actuated" joint in the wbi is mapped to an axes of a yarp controlboard
#torso
#left_leg
#right_leg

#joints
#IMU
#FTS

[WBI_STATE_OPTIONS]
WORLD_REFERENCE_FRAME codyco_balancing_world
estimateBasePosAndVel

[WBI_YARP_JOINTS]
# WBI_JOINT_ID = (YARP_CONTROLBOARD_NAME,YARP_CONTROLBOARD_AXIS)
# The port prefix of the controlboard is obtained as /${robot}/${YARP_CONTROLBOARD_NAME}
torso_yaw        = (torso,0)
torso_roll       = (torso,1)
torso_pitch      = (torso,2)

neck_pitch       = (head,0)
neck_roll        = (head,1)
neck_yaw         = (head,2)

l_shoulder_pitch = (left_arm,0)
l_shoulder_roll  = (left_arm,1)
l_shoulder_yaw   = (left_arm,2)
l_elbow          = (left_arm,3)
l_wrist_prosup   = (left_arm,4)
l_wrist_pitch    = (left_arm,5)
l_wrist_yaw      = (left_arm,6)

r_shoulder_pitch = (right_arm,0)
r_shoulder_roll  = (right_arm,1)
r_shoulder_yaw   = (right_arm,2)
r_elbow          = (right_arm,3)
r_wrist_prosup   = (right_arm,4)
r_wrist_pitch    = (right_arm,5)
r_wrist_yaw      = (right_arm,6)

l_hip_pitch      = (left_leg,0)
l_hip_roll       = (left_leg,1)
l_hip_yaw        = (left_leg,2)
l_knee           = (left_leg,3)
l_ankle_pitch    = (left_leg,4)
l_ankle_roll     = (left_leg,5)

r_hip_pitch      = (right_leg,0)
r_hip_roll       = (right_leg,1)
r_hip_yaw        = (right_leg,2)
r_knee           = (right_leg,3)
r_ankle_pitch    = (right_leg,4)
r_ankle_roll     = (right_leg,5)

[WBI_YARP_IMU_PORTS]
imu_frame /inertial

[WBI_YARP_FT_PORTS]
l_arm_ft_sensor /left_arm/analog:o
r_arm_ft_sensor /right_arm/analog:o
l_leg_ft_sensor /left_leg/analog:o
l_foot_ft_sensor /left_foot/analog:o
r_leg_ft_sensor /right_leg/analog:o
r_foot_ft_sensor /right_foot/analog:o

[WBI_ID_LISTS]
# List of joints of the joints of the robot for various uses
ROBOT_TORSO_JOINTS          = (torso_pitch,torso_roll,torso_yaw)

ROBOT_HEAD_JOINTS           = (neck_pitch, neck_roll, neck_yaw)

ROBOT_LEFT_ARM_JOINTS       = (l_shoulder_pitch, l_shoulder_roll, l_shoulder_yaw, l_elbow, l_wrist_prosup)
ROBOT_RIGHT_ARM_JOINTS      = (r_shoulder_pitch, r_shoulder_roll, r_shoulder_yaw, r_elbow, r_wrist_prosup)

ROBOT_LEFT_ARM_DYNAMIC_MODEL_JOINTS = (ROBOT_LEFT_ARM_JOINTS,l_wrist_pitch,l_wrist_yaw)
ROBOT_RIGHT_ARM_DYNAMIC_MODEL_JOINTS = (ROBOT_RIGHT_ARM_JOINTS,r_wrist_pitch,r_wrist_yaw)

ROBOT_LEFT_LEG_JOINTS       = (l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll)
#ROBOT_LEFT_LEG_JOINTS       = ()

ROBOT_RIGHT_LEG_JOINTS      = (r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)

#ROBOT_DYNAMIC_MODEL_JOINTS  = (ROBOT_TORSO_JOINTS, ROBOT_HEAD_JOINTS, ROBOT_LEFT_ARM_DYNAMIC_MODEL_JOINTS, ROBOT_RIGHT_ARM_DYNAMIC_MODEL_JOINTS, ROBOT_LEFT_LEG_JOINTS, ROBOT_RIGHT_LEG_JOINTS)
ROBOT_DYNAMIC_MODEL_JOINTS  = (ROBOT_TORSO_JOINTS, ROBOT_HEAD_JOINTS, ROBOT_LEFT_ARM_DYNAMIC_MODEL_JOINTS, ROBOT_RIGHT_ARM_DYNAMIC_MODEL_JOINTS, ROBOT_RIGHT_LEG_JOINTS)

ROBOT_MAIN_JOINTS           = (ROBOT_TORSO_JOINTS,ROBOT_LEFT_ARM_JOINTS, ROBOT_RIGHT_ARM_JOINTS,ROBOT_LEFT_LEG_JOINTS,ROBOT_RIGHT_LEG_JOINTS )

ROBOT_TORQUE_CONTROL_JOINTS = (ROBOT_TORSO_JOINTS,ROBOT_LEFT_ARM_JOINTS, ROBOT_RIGHT_ARM_JOINTS,ROBOT_LEFT_LEG_JOINTS,ROBOT_RIGHT_LEG_JOINTS )
ROBOT_TORQUE_CONTROL_JOINTS_WITHOUT_PRONOSUP = (ROBOT_TORSO_JOINTS,l_shoulder_pitch, l_shoulder_roll, l_shoulder_yaw, l_elbow, r_shoulder_pitch, r_shoulder_roll, r_shoulder_yaw, r_elbow,ROBOT_LEFT_LEG_JOINTS,ROBOT_RIGHT_LEG_JOINTS )

# List of imus of the robot
ROBOT_MAIN_IMUS             = (imu_frame)

# List of 6-axes ft sensors of the robot
ROBOT_LEFT_ARM_FTS          = (l_arm_ft_sensor)
ROBOT_RIGHT_ARM_FTS         = (r_arm_ft_sensor)

#ROBOT_LEFT_LEG_FTS          = (l_leg_ft_sensor,l_foot_ft_sensor)
ROBOT_LEFT_LEG_FTS          = ()

ROBOT_RIGHT_LEG_FTS         = (r_leg_ft_sensor,r_foot_ft_sensor)
ROBOT_MAIN_FTS              = (ROBOT_LEFT_ARM_FTS,ROBOT_RIGHT_ARM_FTS,ROBOT_LEFT_LEG_FTS,ROBOT_RIGHT_LEG_FTS)

output:

icub@icub-desktop:~$ wholeBodyDynamicsTree --autoconnect
||| clearing context
||| adding context [wholeBodyDynamicsTree]
||| configuring
||| no policy found
||| default config file specified as wholeBodyDynamicsTree.ini
||| checking [/home/icub/wholeBodyDynamicsTree.ini] (pwd)
||| checking [/home/icub/.config/yarp/robots/iCubParis02] (robot YARP_CONFIG_HOME)
||| checking [/home/icub/.local/share/yarp/robots/iCubParis02] (robot YARP_DATA_HOME)
||| found /home/icub/.local/share/yarp/robots/iCubParis02
||| checking [/etc/yarp/robots/iCubParis02] (robot YARP_CONFIG_DIRS)
||| checking [/usr/local/src/robot//yarp/build-x86_64/share/yarp/robots/iCubParis02] (robot YARP_DATA_DIRS)
||| checking [/usr/local/src/robot//icub-main/build-x86_64/share/iCub/robots/iCubParis02] (robot YARP_DATA_DIRS)
||| found /usr/local/src/robot//icub-main/build-x86_64/share/iCub/robots/iCubParis02
||| checking [/usr/local/src/robot//misc/iCubContrib/share/ICUBcontrib/robots/iCubParis02] (robot YARP_DATA_DIRS)
||| checking [/usr/local/src/robot//misc/wysiwyd/main/build/share/wysiwyd/robots/iCubParis02] (robot YARP_DATA_DIRS)
||| checking [/usr/local/src/robot//codyco-superbuild/build/install/share/codyco/robots/iCubParis02] (robot YARP_DATA_DIRS)
||| found /usr/local/src/robot//codyco-superbuild/build/install/share/codyco/robots/iCubParis02
||| checking [/usr/local/src/robot//yarp/build-x86_64/share/yarp/config/path.d] (robot path.d YARP_DATA_DIRS)
||| checking [/usr/local/src/robot//icub-main/build-x86_64/share/iCub/config/path.d] (robot path.d YARP_DATA_DIRS)
||| checking [/usr/local/src/robot//misc/iCubContrib/share/ICUBcontrib/config/path.d] (robot path.d YARP_DATA_DIRS)
||| checking [/usr/local/src/robot//misc/wysiwyd/main/build/share/wysiwyd/config/path.d] (robot path.d YARP_DATA_DIRS)
||| checking [/usr/local/src/robot//codyco-superbuild/build/install/share/codyco/config/path.d] (robot path.d YARP_DATA_DIRS)
||| checking [/home/icub/.local/share/yarp/robots/iCubParis02/wholeBodyDynamicsTree.ini] (robot)
||| checking [/usr/local/src/robot//icub-main/build-x86_64/share/iCub/robots/iCubParis02/wholeBodyDynamicsTree.ini] (robot)
||| checking [/usr/local/src/robot//codyco-superbuild/build/install/share/codyco/robots/iCubParis02/wholeBodyDynamicsTree.ini] (robot)
||| found /usr/local/src/robot//codyco-superbuild/build/install/share/codyco/robots/iCubParis02/wholeBodyDynamicsTree.ini
yarp: Port /wholeBodyDynamicsTree/rpc:i active at tcp://10.0.0.40:11066
||| finding file [wbi_conf_file]
||| checking [/home/icub/yarpWholeBodyInterface.ini] (pwd)
||| checking [/home/icub/.local/share/yarp/robots/iCubParis02/yarpWholeBodyInterface.ini] (robot)
||| checking [/usr/local/src/robot//icub-main/build-x86_64/share/iCub/robots/iCubParis02/yarpWholeBodyInterface.ini] (robot)
||| checking [/usr/local/src/robot//codyco-superbuild/build/install/share/codyco/robots/iCubParis02/yarpWholeBodyInterface.ini] (robot)
||| found /usr/local/src/robot//codyco-superbuild/build/install/share/codyco/robots/iCubParis02/yarpWholeBodyInterface.ini
[INFO] yarpWholeBodySensors: initializing with 26 encoders 0 pwm sensors 4 F/T sensors 1 IMUs 
[INFO]RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /wholeBodyDynamicsTree/torso/rpc:o active at tcp://10.0.0.40:11067
yarp: Port /wholeBodyDynamicsTree/torso/command:o active at tcp://10.0.0.40:11068
yarp: Port /wholeBodyDynamicsTree/torso/state:i active at tcp://10.0.0.40:11069
yarp: Port /wholeBodyDynamicsTree/torso/stateExt:i active at tcp://10.0.0.40:11070
yarp: Sending output from /wholeBodyDynamicsTree/torso/rpc:o to /icub/torso/rpc:i using tcp
yarp: Datagram packet size set to 65000
yarp: Sending output from /wholeBodyDynamicsTree/torso/command:o to /icub/torso/command:i using udp
yarp: Datagram packet size set to 65000
yarp: Receiving input from /icub/torso/state:o to /wholeBodyDynamicsTree/torso/state:i using udp
yarp: Datagram packet size set to 65000
yarp: Receiving input from /icub/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/head/rpc:o active at tcp://10.0.0.40:11071
yarp: Port /wholeBodyDynamicsTree/head/command:o active at tcp://10.0.0.40:11072
yarp: Port /wholeBodyDynamicsTree/head/state:i active at tcp://10.0.0.40:11073
yarp: Port /wholeBodyDynamicsTree/head/stateExt:i active at tcp://10.0.0.40:11074
yarp: Sending output from /wholeBodyDynamicsTree/head/rpc:o to /icub/head/rpc:i using tcp
yarp: Datagram packet size set to 65000
yarp: Sending output from /wholeBodyDynamicsTree/head/command:o to /icub/head/command:i using udp
yarp: Datagram packet size set to 65000
yarp: Receiving input from /icub/head/state:o to /wholeBodyDynamicsTree/head/state:i using udp
yarp: Datagram packet size set to 65000
yarp: Receiving input from /icub/head/stateExt:o to /wholeBodyDynamicsTree/head/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_arm/rpc:o active at tcp://10.0.0.40:11075
yarp: Port /wholeBodyDynamicsTree/left_arm/command:o active at tcp://10.0.0.40:11076
yarp: Port /wholeBodyDynamicsTree/left_arm/state:i active at tcp://10.0.0.40:11077
yarp: Port /wholeBodyDynamicsTree/left_arm/stateExt:i active at tcp://10.0.0.40:11078
yarp: Sending output from /wholeBodyDynamicsTree/left_arm/rpc:o to /icub/left_arm/rpc:i using tcp
yarp: Datagram packet size set to 65000
yarp: Sending output from /wholeBodyDynamicsTree/left_arm/command:o to /icub/left_arm/command:i using udp
yarp: Datagram packet size set to 65000
yarp: Receiving input from /icub/left_arm/state:o to /wholeBodyDynamicsTree/left_arm/state:i using udp
yarp: Datagram packet size set to 65000
yarp: Receiving input from /icub/left_arm/stateExt:o to /wholeBodyDynamicsTree/left_arm/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_arm/rpc:o active at tcp://10.0.0.40:11079
yarp: Port /wholeBodyDynamicsTree/right_arm/command:o active at tcp://10.0.0.40:11080
yarp: Port /wholeBodyDynamicsTree/right_arm/state:i active at tcp://10.0.0.40:11081
yarp: Port /wholeBodyDynamicsTree/right_arm/stateExt:i active at tcp://10.0.0.40:11082
yarp: Sending output from /wholeBodyDynamicsTree/right_arm/rpc:o to /icub/right_arm/rpc:i using tcp
yarp: Datagram packet size set to 65000
yarp: Sending output from /wholeBodyDynamicsTree/right_arm/command:o to /icub/right_arm/command:i using udp
yarp: Datagram packet size set to 65000
yarp: Receiving input from /icub/right_arm/state:o to /wholeBodyDynamicsTree/right_arm/state:i using udp
yarp: Datagram packet size set to 65000
yarp: Receiving input from /icub/right_arm/stateExt:o to /wholeBodyDynamicsTree/right_arm/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://10.0.0.40:11087
yarp: Port /wholeBodyDynamicsTree/right_leg/command:o active at tcp://10.0.0.40:11088
yarp: Port /wholeBodyDynamicsTree/right_leg/state:i active at tcp://10.0.0.40:11089
yarp: Port /wholeBodyDynamicsTree/right_leg/stateExt:i active at tcp://10.0.0.40:11090
yarp: Sending output from /wholeBodyDynamicsTree/right_leg/rpc:o to /icub/right_leg/rpc:i using tcp
yarp: Datagram packet size set to 65000
yarp: Sending output from /wholeBodyDynamicsTree/right_leg/command:o to /icub/right_leg/command:i using udp
yarp: Datagram packet size set to 65000
yarp: Receiving input from /icub/right_leg/state:o to /wholeBodyDynamicsTree/right_leg/state:i using udp
yarp: Datagram packet size set to 65000
yarp: Receiving input from /icub/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_arm_ft_sensor:i active at tcp://10.0.0.40:11091
yarp: Datagram packet size set to 65000
yarp: Receiving input from /icub/left_arm/analog:o to /wholeBodyDynamicsTree/ftSens/l_arm_ft_sensor:i using udp
yarp: Port /wholeBodyDynamicsTree/ftSens/r_arm_ft_sensor:i active at tcp://10.0.0.40:11092
yarp: Datagram packet size set to 65000
yarp: Receiving input from /icub/right_arm/analog:o to /wholeBodyDynamicsTree/ftSens/r_arm_ft_sensor:i using udp
yarp: Port /wholeBodyDynamicsTree/ftSens/r_leg_ft_sensor:i active at tcp://10.0.0.40:11094
yarp: Datagram packet size set to 65000
yarp: Receiving input from /icub/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://10.0.0.40:11095
yarp: Datagram packet size set to 65000
yarp: Receiving input from /icub/right_foot/analog:o to /wholeBodyDynamicsTree/ftSens/r_foot_ft_sensor:i using udp
yarp: Port /wholeBodyDynamicsTree/imu/imu_frame:i active at tcp://10.0.0.40:11096
yarp: Datagram packet size set to 65000
yarp: Receiving input from /icub/inertial to /wholeBodyDynamicsTree/imu/imu_frame:i using udp
[INFO]Launching wholeBodyDynamicsThread with name :  wholeBodyDynamicsTree  and robotName  icub  and period  10 
[INFO] TorqueEstimationTree constructor: loaded urdf with 26dofs and 4 fts ( 4) 
[FATAL]Assertion failure (dof_serialization.size() == serial.getNrOfDOFs())
/usr/local/src/robot/yarp/build-x86_64/lib/libYARP_OS.so.1(_ZNK4yarp2os3Log5fatalEPKcz+0x143) [0x7fdffce36173]
/usr/local/src/robot/codyco-superbuild/build/install/lib/x86_64-linux-gnu/libidyntree-yarp.so.0.3.5(_ZN4iCub8iDynTree20TorqueEstimationTree27TorqueEstimationConstructorERN3KDL4TreeESt6vectorIN13kdl_format_io12FTSensorDataESaIS7_EES5_ISsSaISsEESB_RN4yarp3sig6VectorESF_Ssj+0x553) [0x7fdffa8fdebb]
/usr/local/src/robot/codyco-superbuild/build/install/lib/x86_64-linux-gnu/libidyntree-yarp.so.0.3.5(_ZN4iCub8iDynTree20TorqueEstimationTreeC1ESsSt6vectorISsSaISsEES4_Ssj+0x44e) [0x7fdffa8fd284]
wholeBodyDynamicsTree(_ZN23wholeBodyDynamicsThreadC2ESsSsiPN7yarpWbi20yarpWholeBodySensorsERN4yarp2os8PropertyEbSsb+0xc48) [0x4338d8]
wholeBodyDynamicsTree(_ZN23wholeBodyDynamicsModule9configureERN4yarp2os14ResourceFinderE+0x21b4) [0x468a54]
/usr/local/src/robot/yarp/build-x86_64/lib/libYARP_OS.so.1(_ZN4yarp2os8RFModule9runModuleERNS0_14ResourceFinderE+0xa) [0x7fdffceb489a]
wholeBodyDynamicsTree(main+0x7b4) [0x4275a4]
/lib/x86_64-linux-gnu/libc.so.6(__libc_start_main+0xf5) [0x7fdff6d87b45]
wholeBodyDynamicsTree() [0x42848e]
icub@icub-desktop:~$

traversaro commented 9 years ago

Ah.. sorry, I forgot that I had hardcoded that assumption that the number of joint used for estimation is the same of the URDF.. this should be definitely be fixed (ref: https://github.com/robotology-playground/idyntree/issues/48 ).

The easiest workaround for now is to change the type of all the joints belonging to the left leg in the urdf used by the yarpWholeBodyInterface (in https://github.com/robotology-playground/yarp-wholebodyinterface/tree/master/app/robots/iCubParis02) from revolute to fixed.

rlober commented 9 years ago

Ok so I finally got it working and yes running wholeBodyDynamicsTree --autocomplete fixed the HARWARE FAULT issue.

Summary of the solution: Given that parts have been commented out in icub_all.xml

In yarpWholeBodyInterface.ini remove these parts in ROBOT_DYNAMIC_MODEL_JOINTS and in ROBOT_MAIN_FTS
In wholeBodyDynamicsTree.ini remove the parts in WBD_OUTPUT_TORQUE_PORTS and WBD_SUBTREES
In model.urdf change the joints of the part from revolute to fixed. As @traversaro mentioned - this will not be necessary in the future.

Below I have put the files for your reference. The files that are loaded are typically located under $CODYCO_SUPERBUILD_ROOT/build/install/share/codyco/robots/[your_robot_name]/. So you need to modify their sources for this to take permanent effect.

You can see the files I have modified for iCubParis02 here:

wholeBodyDynamicsTree_no_left_leg.ini

yarpWholeBodyInterface_no_left_leg.ini

model_fixed_left_leg.urdf

Once you have rebuilt superbuild with these files you just run:

wholeBodyDynamicsTree --from wholeBodyDynamicsTree_no_left_leg.ini --autoconnect

traversaro commented 9 years ago

:+1:

robotology / icub-main

Hardware Fault on switch to Torque Mode #165