reconfirm inertia params

This commit is contained in:
TrivasZhang 2022-02-11 15:55:18 +08:00
parent 7325aa56ec
commit 49e936b9a8
4 changed files with 88 additions and 86 deletions

View File

@ -49,7 +49,7 @@
<xacro:property name="calf_velocity_max" value="21"/> <xacro:property name="calf_velocity_max" value="21"/>
<xacro:property name="calf_torque_max" value="33.5"/> <xacro:property name="calf_torque_max" value="33.5"/>
<!-- dynamics inertial value --> <!-- dynamics inertial value total 13.9kg -->
<!-- trunk --> <!-- trunk -->
<xacro:property name="trunk_mass" value="6.0"/> <xacro:property name="trunk_mass" value="6.0"/>
<xacro:property name="trunk_com_x" value="0.0000"/> <xacro:property name="trunk_com_x" value="0.0000"/>
@ -63,40 +63,40 @@
<xacro:property name="trunk_izz" value="0.0456542"/> <xacro:property name="trunk_izz" value="0.0456542"/>
<!-- hip (left front) --> <!-- hip (left front) -->
<xacro:property name="hip_mass" value="0.696"/> <xacro:property name="hip_mass" value="0.595"/>
<xacro:property name="hip_com_x" value="-0.003311"/> <xacro:property name="hip_com_x" value="-0.003875"/>
<xacro:property name="hip_com_y" value="0.000635"/> <xacro:property name="hip_com_y" value="0.001622"/>
<xacro:property name="hip_com_z" value="0.000031"/> <xacro:property name="hip_com_z" value="0.000042"/>
<xacro:property name="hip_ixx" value="0.000469246"/> <xacro:property name="hip_ixx" value="0.000402747"/>
<xacro:property name="hip_ixy" value="-0.000009409"/> <xacro:property name="hip_ixy" value="-0.000008709"/>
<xacro:property name="hip_ixz" value="-0.000000342"/> <xacro:property name="hip_ixz" value="-0.000000297"/>
<xacro:property name="hip_iyy" value="0.000807490"/> <xacro:property name="hip_iyy" value="0.000691123"/>
<xacro:property name="hip_iyz" value="-0.000000466"/> <xacro:property name="hip_iyz" value="-0.000000545"/>
<xacro:property name="hip_izz" value="0.000552929"/> <xacro:property name="hip_izz" value="0.000487919"/>
<!-- thigh --> <!-- thigh -->
<xacro:property name="thigh_mass" value="1.013"/> <xacro:property name="thigh_mass" value="0.888"/>
<xacro:property name="thigh_com_x" value="-0.003237"/> <xacro:property name="thigh_com_x" value="-0.003574"/>
<xacro:property name="thigh_com_y" value="-0.022327"/> <xacro:property name="thigh_com_y" value="-0.019529"/>
<xacro:property name="thigh_com_z" value="-0.027326"/> <xacro:property name="thigh_com_z" value="-0.030323"/>
<xacro:property name="thigh_ixx" value="0.005529065"/> <xacro:property name="thigh_ixx" value="0.005251806"/>
<xacro:property name="thigh_ixy" value="0.000004825"/> <xacro:property name="thigh_ixy" value="-0.000002168"/>
<xacro:property name="thigh_ixz" value="0.000343869"/> <xacro:property name="thigh_ixz" value="0.000346889"/>
<xacro:property name="thigh_iyy" value="0.005139339"/> <xacro:property name="thigh_iyy" value="0.005000475"/>
<xacro:property name="thigh_iyz" value="0.000022448"/> <xacro:property name="thigh_iyz" value="-0.000028174"/>
<xacro:property name="thigh_izz" value="0.001367788"/> <xacro:property name="thigh_izz" value="0.001110200"/>
<!-- calf --> <!-- calf -->
<xacro:property name="calf_mass" value="0.166"/> <xacro:property name="calf_mass" value="0.151"/>
<xacro:property name="calf_com_x" value="0.006435"/> <xacro:property name="calf_com_x" value="0.007105"/>
<xacro:property name="calf_com_y" value="0.0"/> <xacro:property name="calf_com_y" value="-0.000239"/>
<xacro:property name="calf_com_z" value="-0.107388"/> <xacro:property name="calf_com_z" value="-0.096933"/>
<xacro:property name="calf_ixx" value="0.002997972"/> <xacro:property name="calf_ixx" value="0.002344758"/>
<xacro:property name="calf_ixy" value="0.0"/> <xacro:property name="calf_ixy" value="0.0"/>
<xacro:property name="calf_ixz" value="-0.000141163"/> <xacro:property name="calf_ixz" value="-0.000141275"/>
<xacro:property name="calf_iyy" value="0.003014022"/> <xacro:property name="calf_iyy" value="0.002360755"/>
<xacro:property name="calf_iyz" value="0.0"/> <xacro:property name="calf_iyz" value="0.0"/>
<xacro:property name="calf_izz" value="0.000032426"/> <xacro:property name="calf_izz" value="0.000031158"/>
<!-- foot --> <!-- foot -->
<xacro:property name="foot_mass" value="0.06"/> <xacro:property name="foot_mass" value="0.06"/>

View File

@ -23,12 +23,12 @@
<xacro:property name="stick_length" value="0.2"/> <xacro:property name="stick_length" value="0.2"/>
<!-- kinematic value --> <!-- kinematic value -->
<xacro:property name="thigh_offset" value="0.083"/> <xacro:property name="thigh_offset" value="0.0868"/>
<xacro:property name="thigh_length" value="0.25"/> <xacro:property name="thigh_length" value="0.25"/>
<xacro:property name="calf_length" value="0.25"/> <xacro:property name="calf_length" value="0.25"/>
<!-- leg offset from trunk center value --> <!-- leg offset from trunk center value -->
<xacro:property name="leg_offset_x" value="0.2399"/> <xacro:property name="leg_offset_x" value="0.2407"/>
<xacro:property name="leg_offset_y" value="0.051"/> <xacro:property name="leg_offset_y" value="0.051"/>
<xacro:property name="trunk_offset_z" value="0.01675"/> <xacro:property name="trunk_offset_z" value="0.01675"/>
<xacro:property name="hip_offset" value="0.083"/> <xacro:property name="hip_offset" value="0.083"/>
@ -39,15 +39,17 @@
<xacro:property name="hip_position_max" value="${70*PI/180.0}"/> <xacro:property name="hip_position_max" value="${70*PI/180.0}"/>
<xacro:property name="hip_position_min" value="${-70*PI/180.0}"/> <xacro:property name="hip_position_min" value="${-70*PI/180.0}"/>
<xacro:property name="hip_velocity_max" value="20"/> <xacro:property name="hip_velocity_max" value="20"/>
<xacro:property name="hip_torque_max" value="44"/> <xacro:property name="hip_torque_max" value="35.278"/>
<xacro:property name="thigh_position_max" value="${240*PI/180.0}"/>
<xacro:property name="thigh_position_min" value="${-120*PI/180.0}"/>
<xacro:property name="thigh_velocity_max" value="20"/> <xacro:property name="thigh_velocity_max" value="20"/>
<xacro:property name="thigh_torque_max" value="44"/> <xacro:property name="thigh_torque_max" value="35.278"/>
<xacro:property name="calf_position_max" value="${-37*PI/180.0}"/> <xacro:property name="calf_position_max" value="${-37*PI/180.0}"/>
<xacro:property name="calf_position_min" value="${-159*PI/180.0}"/> <xacro:property name="calf_position_min" value="${-159*PI/180.0}"/>
<xacro:property name="calf_velocity_max" value="16"/> <xacro:property name="calf_velocity_max" value="15.89"/>
<xacro:property name="calf_torque_max" value="55"/> <xacro:property name="calf_torque_max" value="44.4"/>
<!-- dynamics inertial value --> <!-- dynamics inertial value total 22.0kg -->
<!-- trunk --> <!-- trunk -->
<xacro:property name="trunk_mass" value="11.041"/> <xacro:property name="trunk_mass" value="11.041"/>
<xacro:property name="trunk_com_x" value="0.008465"/> <xacro:property name="trunk_com_x" value="0.008465"/>

View File

@ -36,67 +36,67 @@
<!-- joint limits --> <!-- joint limits -->
<xacro:property name="damping" value="0.01"/> <xacro:property name="damping" value="0.01"/>
<xacro:property name="friction" value="0.2"/> <xacro:property name="friction" value="0.2"/>
<xacro:property name="hip_max" value="46"/> <xacro:property name="hip_max" value="60"/>
<xacro:property name="hip_min" value="-46"/> <xacro:property name="hip_min" value="-60"/>
<xacro:property name="hip_velocity_max" value="21"/> <xacro:property name="hip_velocity_max" value="30.1"/>
<xacro:property name="hip_torque_max" value="33.5"/> <xacro:property name="hip_torque_max" value="23.7"/>
<xacro:property name="thigh_max" value="240"/> <xacro:property name="thigh_max" value="170"/>
<xacro:property name="thigh_min" value="-60"/> <xacro:property name="thigh_min" value="-38"/>
<xacro:property name="thigh_velocity_max" value="21"/> <xacro:property name="thigh_velocity_max" value="30.1"/>
<xacro:property name="thigh_torque_max" value="33.5"/> <xacro:property name="thigh_torque_max" value="23.7"/>
<xacro:property name="calf_max" value="-52.5"/> <xacro:property name="calf_max" value="-48"/>
<xacro:property name="calf_min" value="-154.5"/> <xacro:property name="calf_min" value="-156"/>
<xacro:property name="calf_velocity_max" value="21"/> <xacro:property name="calf_velocity_max" value="30.1"/>
<xacro:property name="calf_torque_max" value="33.5"/> <xacro:property name="calf_torque_max" value="23.7"/>
<!-- dynamics inertial value --> <!-- dynamics inertial value total 12.84kg -->
<!-- trunk --> <!-- trunk -->
<xacro:property name="trunk_mass" value="4.8"/> <xacro:property name="trunk_mass" value="5.204"/>
<xacro:property name="trunk_com_x" value="0.011611"/> <xacro:property name="trunk_com_x" value="0.0223"/>
<xacro:property name="trunk_com_y" value="0.004437"/> <xacro:property name="trunk_com_y" value="0.002"/>
<xacro:property name="trunk_com_z" value="0.000108"/> <xacro:property name="trunk_com_z" value="-0.0005"/>
<xacro:property name="trunk_ixx" value="0.016130741919"/> <xacro:property name="trunk_ixx" value="0.0168352186"/>
<xacro:property name="trunk_ixy" value="0.000593180607"/> <xacro:property name="trunk_ixy" value="0.0004636141"/>
<xacro:property name="trunk_ixz" value="0.000007324662"/> <xacro:property name="trunk_ixz" value="0.0002367952"/>
<xacro:property name="trunk_iyy" value="0.036507810812"/> <xacro:property name="trunk_iyy" value="0.0656071082"/>
<xacro:property name="trunk_iyz" value="0.000020969537"/> <xacro:property name="trunk_iyz" value="0.000036671"/>
<xacro:property name="trunk_izz" value="0.044693872053"/> <xacro:property name="trunk_izz" value="0.0742720659"/>
<!-- hip (left front) --> <!-- hip (left front) -->
<xacro:property name="hip_mass" value="0.510299"/> <xacro:property name="hip_mass" value="0.591"/>
<xacro:property name="hip_com_x" value="-0.00541"/> <xacro:property name="hip_com_x" value="-0.00541"/>
<xacro:property name="hip_com_y" value="-0.00074"/> <xacro:property name="hip_com_y" value="-0.00074"/>
<xacro:property name="hip_com_z" value="0.000006"/> <xacro:property name="hip_com_z" value="0.000006"/>
<xacro:property name="hip_ixx" value="0.00030528937"/> <xacro:property name="hip_ixx" value="0.000374268192"/>
<xacro:property name="hip_ixy" value="-0.000007788013"/> <xacro:property name="hip_ixy" value="0.000036844422"/>
<xacro:property name="hip_ixz" value="0.000000220160"/> <xacro:property name="hip_ixz" value="-0.000000986754"/>
<xacro:property name="hip_iyy" value="0.000590894859"/> <xacro:property name="hip_iyy" value="0.000635923669"/>
<xacro:property name="hip_iyz" value="-0.000000017175"/> <xacro:property name="hip_iyz" value="-0.000001172894"/>
<xacro:property name="hip_izz" value="0.000396594572"/> <xacro:property name="hip_izz" value="0.000457647394"/>
<!-- thigh --> <!-- thigh -->
<xacro:property name="thigh_mass" value="0.898919"/> <xacro:property name="thigh_mass" value="0.92"/>
<xacro:property name="thigh_com_x" value="-0.003468"/> <xacro:property name="thigh_com_x" value="-0.003468"/>
<xacro:property name="thigh_com_y" value="-0.018947"/> <xacro:property name="thigh_com_y" value="-0.018947"/>
<xacro:property name="thigh_com_z" value="-0.032736"/> <xacro:property name="thigh_com_z" value="-0.032736"/>
<xacro:property name="thigh_ixx" value="0.005395867678"/> <xacro:property name="thigh_ixx" value="0.005851561134"/>
<xacro:property name="thigh_ixy" value="0.000000102809"/> <xacro:property name="thigh_ixy" value="0.000001783284"/>
<xacro:property name="thigh_ixz" value="0.000337529085"/> <xacro:property name="thigh_ixz" value="0.000328291374"/>
<xacro:property name="thigh_iyy" value="0.005142451046"/> <xacro:property name="thigh_iyy" value="0.005596155105"/>
<xacro:property name="thigh_iyz" value="-0.000005816563"/> <xacro:property name="thigh_iyz" value="0.000021430713"/>
<xacro:property name="thigh_izz" value="0.00102478732"/> <xacro:property name="thigh_izz" value="0.00107157026"/>
<!-- calf --> <!-- calf -->
<xacro:property name="calf_mass" value="0.158015"/> <xacro:property name="calf_mass" value="0.131"/>
<xacro:property name="calf_com_x" value="0.006286"/> <xacro:property name="calf_com_x" value="0.006286"/>
<xacro:property name="calf_com_y" value="0.001307"/> <xacro:property name="calf_com_y" value="0.001307"/>
<xacro:property name="calf_com_z" value="-0.122269"/> <xacro:property name="calf_com_z" value="-0.122269"/>
<xacro:property name="calf_ixx" value="0.003607648222"/> <xacro:property name="calf_ixx" value="0.002939186297"/>
<xacro:property name="calf_ixy" value="0.000001494971"/> <xacro:property name="calf_ixy" value="0.000001440899"/>
<xacro:property name="calf_ixz" value="-0.000132778525"/> <xacro:property name="calf_ixz" value="-0.000105359550"/>
<xacro:property name="calf_iyy" value="0.003626771492"/> <xacro:property name="calf_iyy" value="0.00295576935"/>
<xacro:property name="calf_iyz" value="-0.000028638535"/> <xacro:property name="calf_iyz" value="-0.000024397752"/>
<xacro:property name="calf_izz" value="0.000035148003"/> <xacro:property name="calf_izz" value="0.000030273372"/>
<!-- foot --> <!-- foot -->
<xacro:property name="foot_mass" value="0.06"/> <xacro:property name="foot_mass" value="0.06"/>

View File

@ -38,16 +38,16 @@
<xacro:property name="friction" value="0"/> <xacro:property name="friction" value="0"/>
<xacro:property name="hip_max" value="60"/> <xacro:property name="hip_max" value="60"/>
<xacro:property name="hip_min" value="-50"/> <xacro:property name="hip_min" value="-50"/>
<xacro:property name="hip_velocity_max" value="52.4"/> <xacro:property name="hip_velocity_max" value="27"/>
<xacro:property name="hip_torque_max" value="20"/> <xacro:property name="hip_torque_max" value="18.954"/>
<xacro:property name="thigh_max" value="225"/> <xacro:property name="thigh_max" value="225"/>
<xacro:property name="thigh_min" value="-30"/> <xacro:property name="thigh_min" value="-30"/>
<xacro:property name="thigh_velocity_max" value="28.6"/> <xacro:property name="thigh_velocity_max" value="12.794"/>
<xacro:property name="thigh_torque_max" value="55"/> <xacro:property name="thigh_torque_max" value="40"/>
<xacro:property name="calf_max" value="-35"/> <xacro:property name="calf_max" value="-35"/>
<xacro:property name="calf_min" value="-159"/> <xacro:property name="calf_min" value="-159"/>
<xacro:property name="calf_velocity_max" value="28.6"/> <xacro:property name="calf_velocity_max" value="12.794"/>
<xacro:property name="calf_torque_max" value="55"/> <xacro:property name="calf_torque_max" value="40"/>
<!-- dynamics inertial value --> <!-- dynamics inertial value -->
<!-- trunk --> <!-- trunk -->