wholebodydynamics.xml

<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">


    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="wholebodydynamics" type="wholebodydynamics">
        <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_prosup,l_wrist_pitch,l_wrist_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_prosup,r_wrist_pitch,r_wrist_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
        <param name="modelFile">model.urdf</param>
        <param name="fixedFrameGravity">(0,0,-9.81)</param>
	<param name="defaultContactFrames">(l_hand,r_hand,root_link,l_foot_front,l_foot_rear,r_foot_front,r_foot_rear,l_lower_leg,r_lower_leg)</param>
        <param name="imuFrameName">imu_frame</param>
        <param name="publishOnROS">true</param>
	<param name="forceTorqueEstimateConfidence">2</param>
        <param name="useJointVelocity">true</param>
        <param name="useJointAcceleration">false</param>
	<param name="streamFilteredFT">true</param>
        <param name="imuFilterCutoffInHz">3.0</param>
        <param name="forceTorqueFilterCutoffInHz">3.0</param>
        <param name="jointVelFilterCutoffInHz">3.0</param> <!-- used if useJointVelocity is set to true -->
        <param name="jointAccFilterCutoffInHz">3.0</param> <!-- used if useJointAcceleration is set to true -->
        <param name="useSkinForContacts">false</param>
        <param name="publishNetExternalWrenches">true</param>
	<!--        <param name="assume_fixed">root_link</param>-->
	<group name="HW_USE_MAS_IMU">
	    <param name="accelerometer">rfeimu_acc</param>
	    <param name="gyroscope">rfeimu_gyro</param>
	</group>


        <group name="GRAVITY_COMPENSATION">
            <param name="enableGravityCompensation">true</param>
            <param name="gravityCompensationBaseLink">root_link</param>
            <param name="gravityCompensationAxesNames">(torso_roll,torso_pitch,torso_yaw,neck_pitch,neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
        </group>


        <group name="WBD_OUTPUT_EXTERNAL_WRENCH_PORTS">
	     <param name="/wholeBodyDynamics/left_arm/cartesianEndEffectorWrench:o">(l_hand,l_hand,root_link)</param>
            <param name="/wholeBodyDynamics/right_arm/cartesianEndEffectorWrench:o">(r_hand,r_hand,root_link)</param>
            <param name="/wholeBodyDynamics/left_leg/cartesianEndEffectorWrench:o">(l_lower_leg,l_lower_leg,root_link)</param>
            <param name="/wholeBodyDynamics/right_leg/cartesianEndEffectorWrench:o">(r_lower_leg,r_lower_leg,root_link)</param>
            <param name="/wholeBodyDynamics/left_foot_front/cartesianEndEffectorWrench:o">(l_foot_front,l_sole,l_sole)</param>
            <param name="/wholeBodyDynamics/left_foot_rear/cartesianEndEffectorWrench:o">(l_foot_rear,l_sole,l_sole)</param>
            <param name="/wholeBodyDynamics/right_foot_front/cartesianEndEffectorWrench:o">(r_foot_front,r_sole,r_sole)</param>
            <param name="/wholeBodyDynamics/right_foot_rear/cartesianEndEffectorWrench:o">(r_foot_rear,r_sole,r_sole)</param>
        </group>

        <action phase="startup" level="15" type="attach">
            <paramlist name="networks">
                <!-- motorcontrol and virtual torque sensors -->
		<elem name="head-j0">head-eb20-j0_1-mc</elem>
                <elem name="head-j2">head-eb21-j2_3-mc</elem>
                <elem name="torso">torso-eb5-j0_2-mc</elem>
                <elem name="left_upper_arm-j0">left_arm-eb1-j0_1-mc</elem>
                <elem name="left_upper_arm-j2">left_arm-eb2-j2_3-mc</elem>
                <elem name="left_lower_arm">left_arm-eb24-j4_7-mc</elem>
                <elem name="right_upper_arm-j0">right_arm-eb3-j0_1-mc</elem>
                <elem name="right_upper_arm-j2">right_arm-eb4-j2_3-mc</elem>
                <elem name="right_lower_arm">right_arm-eb27-j4_7-mc</elem>
                <elem name="left_upper_leg-j0">left_leg-eb7-j0_2-mc</elem>
                <elem name="left_lower_leg-j2">left_leg-eb8-j3_5-mc</elem>
                <elem name="right_upper_leg-j0">right_leg-eb11-j0_2-mc</elem>
                <elem name="right_lower_leg-j2">right_leg-eb12-j3_5-mc</elem>

                <!-- imu -->
                <elem name="imu">head-inertial</elem>
                <!-- imu_waist -->
<!--            <elem name="imu">xsensmt-inertial</elem> -->
                <!-- ft -->
                <!--
                <elem name="l_leg_ft_sensor">left_leg-eb6-j0_3-strain</elem>
                <elem name="r_leg_ft_sensor">right_leg-eb8-j0_3-strain</elem> -->

  		 <elem name="l_arm_ft_sensor">left_arm-eb1-j0_1-strain</elem>
                <elem name="r_arm_ft_sensor">right_arm-eb3-j0_1-strain</elem>
                <elem name="l_foot_front_ft_sensor">left_leg-eb8-j3_5-strain</elem>
                <elem name="l_foot_rear_ft_sensor">left_leg-eb7-j0_2-strain</elem>
                <elem name="r_foot_front_ft_sensor">right_leg-eb12-j3_5-strain</elem>
		<elem name="r_foot_rear_ft_sensor">right_leg-eb11-j0_2-strain</elem>
            </paramlist>
        </action>

	<action phase="shutdown" level="2" type="detach" />

    </device>