-
Notifications
You must be signed in to change notification settings - Fork 12
/
wholebodydynamics-icub-external-six-fts-sim.xml
138 lines (129 loc) · 9.34 KB
/
wholebodydynamics-icub-external-six-fts-sim.xml
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
<devices robot="@WBD_YARP_ROBOT_NAME@" build="1">
<device 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_sole,r_sole,l_upper_leg,r_upper_leg,l_elbow_1,r_elbow_1)</param>
<param name="imuFrameName">head_imu_0</param>
<param name="useJointVelocity">true</param>
<param name="useJointAcceleration">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="startWithZeroFTSensorOffsets">true</param> <!-- bypass using resetOffset of FT sensors in simulation -->
<param name="useSkinForContacts">true</param> <!-- bypass handling skinDynLib for robots without skin -->
<param name="publishNetExternalWrenches">true</param>
<param name="devicePeriodInSeconds">0.01</param>
<param name="estimateJointVelocityAcceleration">false</param> <!-- if true a kf will estimate the joint velocities and accelerations -->
<param name="processNoiseCovariance">(1.0e-3, 1.0e-3, 1.0e-3,
1.0e-3, 1.0e-3, 1.0e-3,
1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
1.0e-3, 1.0e-3, 1.0e-3,
1.0e-3, 1.0e-3, 1.0e-3,
1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
1.0e+1, 1.0e+1, 1.0e+1,
1.0e+1, 1.0e+1, 1.0e+1,
1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1,
1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1,
1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1,
1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1)</param>
<param name="measurementNoiseCovariance">(1.0e-3, 1.0e-3, 1.0e-3,
1.0e-3, 1.0e-3, 1.0e-3,
1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3)</param>
<param name="stateCovariance">(5.0e-4, 5.0e-4, 5.0e-4,
5.0e-4, 5.0e-4, 5.0e-4,
5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
1.0e-3, 1.0e-3, 1.0e-3,
1.0e-3, 1.0e-3, 1.0e-3,
1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
1.0e-1, 1.0e-1, 1.0e-1,
1.0e-1, 1.0e-1, 1.0e-1,
1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1,
1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1,
1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1,
1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1)</param>
<!-- map between iDynTree links (identified by a name)
and skinDynLib links (identified by their frame name, a BodyPart enum
and a local (to the body part) index -->
<group name="IDYNTREE_SKINDYNLIB_LINKS">
<param name="root_link">(root_link,1,0)</param>
<param name="chest"> (chest,1,2)</param>
<param name="l_upper_arm">(l_upper_arm,3,2)</param>
<param name="l_elbow_1">(l_elbow_1, 3, 4)</param>
<param name="r_upper_arm">(r_upper_arm,4,2)</param>
<param name="l_hand">(l_hand_dh_frame,3,6)</param>
<param name="r_elbow_1">(r_elbow_1, 4, 4)</param>
<param name="r_hand">(r_hand_dh_frame,4,6)</param>
<param name="l_upper_leg">(l_upper_leg,5,2)</param>
<param name="l_lower_leg">(l_lower_leg,5,3)</param>
<param name="l_ankle_1">(l_ankle_1,5,4)</param>
<param name="l_foot">(l_foot_dh_frame,5,5)</param>
<param name="r_upper_leg">(r_upper_leg,6,2)</param>
<param name="r_lower_leg">(r_lower_leg,6,3)</param>
<param name="r_ankle_1">(r_ankle_1,6,4)</param>
<param name="r_foot">(r_foot_dh_frame,6,5)</param>
</group>
<group name="GRAVITY_COMPENSATION">
<param name="enableGravityCompensation">true</param>
<param name="gravityCompensationBaseLink">root_link</param>
<param name="gravityCompensationAxesNames">(torso_pitch,torso_roll,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)</param>
</group>
<group name="WBD_OUTPUT_EXTERNAL_WRENCH_PORTS">
<param name="/wholeBodyDynamics/left_arm/endEffectorWrench:o">(l_hand,l_hand_dh_frame)</param>
<param name="/wholeBodyDynamics/right_arm/endEffectorWrench:o">(r_hand,r_hand_dh_frame)</param>
<param name="/wholeBodyDynamics/left_leg/cartesianEndEffectorWrench:o">(l_foot,l_sole,root_link)</param>
<param name="/wholeBodyDynamics/right_leg/cartesianEndEffectorWrench:o">(r_foot,r_sole,root_link)</param>
<param name="/wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o">(l_foot,l_sole,l_sole)</param>
<param name="/wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o">(r_foot,r_sole,r_sole)</param>
</group>
<group name="HW_USE_MAS_IMU">
<param name="accelerometer">head_imu_0</param>
<param name="gyroscope">head_imu_0</param>
</group>
<group name="multipleAnalogSensorsNames">
<param name="SixAxisForceTorqueSensorsNames">("l_arm_ft", "r_arm_ft", "l_leg_ft", "r_leg_ft", "l_foot_ft", "r_foot_ft")</param>
</group>
<action phase="startup" level="15" type="attach">
<paramlist name="networks">
<!-- motorcontrol -->
<elem name="left_leg">left_leg_mc</elem>
<elem name="right_leg">right_leg_mc</elem>
<elem name="torso">torso_mc</elem>
<elem name="right_arm">right_arm_mc</elem>
<elem name="left_arm">left_arm_mc</elem>
<elem name="head">head_mc</elem>
<!-- imu -->
<elem name="imu">head-inertial-client</elem>
<!-- ft -->
<elem name="l_arm_ft">left_arm_client</elem>
<elem name="r_arm_ft">right_arm_client</elem>
<elem name="l_leg_ft">left_leg_client</elem>
<elem name="r_leg_ft">right_leg_client</elem>
</paramlist>
</action>
<action phase="shutdown" level="2" type="detach" />
</device>
</devices>