Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[ergoCubSN000] Reenable the torso pitch, increases the joints limits of the shoulders pitch and the prevent fingers from overcurrent #537

Merged
merged 3 commits into from
Jul 6, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion ergoCubSN000/calibrators/torso-calib.xml
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@

<!-- Joint 1 has been removed from the calibration order for motor safety -->
<!-- <param name="CALIB_ORDER">(0) (1) (2) </param> --> <!-- Don't remove this line -->
<param name="CALIB_ORDER">(0) (2) </param>
<param name="CALIB_ORDER">(0) (1) (2) </param>

<action phase="startup" level="10" type="calibrate">
<param name="target">torso-eb5-j0_2-mc</param>
Expand Down
4 changes: 2 additions & 2 deletions ergoCubSN000/estimators/wholebodydynamics.xml
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@


<device xmlns:xi="http://www.w3.org/2001/XInclude" name="wholebodydynamics" type="wholebodydynamics">
<param name="axesNames">(torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_roll,l_wrist_pitch,l_wrist_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_roll,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="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_roll,l_wrist_pitch,l_wrist_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_roll,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_palm,r_hand_palm,root_link,l_foot_front,l_foot_rear,r_foot_front,r_foot_rear,l_upper_leg,r_upper_leg)</param>
Expand Down Expand Up @@ -34,7 +34,7 @@
<group name="GRAVITY_COMPENSATION">
<param name="enableGravityCompensation">true</param>
<param name="gravityCompensationBaseLink">root_link</param>
<param name="gravityCompensationAxesNames">(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,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="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,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>


Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
<!-- joint name shoulder_pitch shoulder_roll -->
<group name="LIMITS">
<param name="jntPosMax"> 14 130 </param>
<param name="jntPosMin"> -88 0 </param>
<param name="jntPosMin"> -130 0 </param>
<param name="jntVelMax"> 120 120 </param>
<param name="motorNominalCurrents"> 5000 5000 </param>
<param name="motorPeakCurrents"> 12000 12000 </param>
Expand Down
4 changes: 2 additions & 2 deletions ergoCubSN000/hardware/motorControl/left_arm-eb23-j7_10-mc.xml
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
<param name="jntPosMax"> 72 85 85 85 </param>
<param name="jntVelMax"> 1000 1000 1000 1000 </param>
<param name="motorOverloadCurrents"> 2000 2000 2000 2000 </param>
<param name="motorNominalCurrents"> 700 700 700 700 </param>
<param name="motorNominalCurrents"> 1100 1100 1100 1100 </param>
<param name="motorPeakCurrents"> 1500 1500 1500 1500 </param>
<param name="motorPwmLimit"> 3360 3360 3360 3360 </param>
</group>
Expand Down Expand Up @@ -51,7 +51,7 @@
<param name="kp"> -200 -200 -200 -200 </param>
<param name="kd"> -10 -10 -10 -10 </param>
<param name="ki"> -10 -10 -10 -10 </param>
<param name="maxOutput"> 2000 2000 2000 2000 </param>
<param name="maxOutput"> 2700 2700 2700 2700 </param>
<param name="maxInt"> 1000 1000 1000 1000 </param>
<param name="stictionUp"> 0 0 0 0 </param>
<param name="stictionDown"> 0 0 0 0 </param>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@
<!-- joint name shoulder_pitch shoulder_roll -->
<group name="LIMITS">
<param name="jntPosMax"> 14 130 </param>
<param name="jntPosMin"> -88 0 </param>
<param name="jntPosMin"> -130 0 </param>
<param name="jntVelMax"> 120 120 </param>
<param name="motorNominalCurrents"> 5000 5000 </param>
<param name="motorPeakCurrents"> 12000 12000 </param>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
<param name="jntPosMax"> 70 63 85 85 </param>
<param name="jntVelMax"> 1000 1000 1000 1000 </param>
<param name="motorOverloadCurrents"> 2000 2000 2000 2000 </param>
<param name="motorNominalCurrents"> 700 700 700 700 </param>
<param name="motorNominalCurrents"> 1100 1100 1100 1100 </param>
<param name="motorPeakCurrents"> 1500 1500 1500 1500 </param>
<param name="motorPwmLimit"> 3360 3360 3360 3360 </param>
</group>
Expand Down Expand Up @@ -51,7 +51,7 @@
<param name="kp"> -200 -200 -200 -200 </param>
<param name="kd"> -10 -10 -10 -10 </param>
<param name="ki"> -10 -10 -10 -10 </param>
<param name="maxOutput"> 2000 2000 2000 2000 </param>
<param name="maxOutput"> 2700 2700 2700 2700 </param>
<param name="maxInt"> 1000 1000 1000 1000 </param>
<param name="stictionUp"> 0 0 0 0 </param>
<param name="stictionDown"> 0 0 0 0 </param>
Expand Down
4 changes: 2 additions & 2 deletions ergoCubSN000/hardware/motorControl/torso-eb5-j0_2-mc.xml
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,8 @@
<param name="jntPosMax"> 23 45 43 </param>
<param name="jntPosMin"> -23 -18 -43 </param>
<param name="jntVelMax"> 120 120 120 </param>
<param name="motorNominalCurrents"> 3770 3770 5000 </param>
<param name="motorPeakCurrents"> 14000 14000 10000 </param>
<param name="motorNominalCurrents"> 3770 16000 5000 </param>
<param name="motorPeakCurrents"> 14000 17000 10000 </param>
<param name="motorOverloadCurrents"> 18000 18000 15000 </param>
<param name="motorPwmLimit"> 16000 16000 16000 </param>
</group>
Expand Down
15 changes: 8 additions & 7 deletions ergoCubSN000/wrappers/motorControl/alljoints-mc_remapper.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,13 +2,14 @@
<!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="alljoints-mc_remapper" type="controlboardremapper">
<!--
These are the regular parameters

<!-- These are the regular parameters -->
<param name="axesNames">(neck_pitch,neck_roll,neck_yaw,camera_tilt,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_yaw,l_wrist_roll,l_wrist_pitch,l_thumb_add,l_thumb_oc,l_index_add,l_index_oc,l_middle_oc,l_ring_pinky_oc,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_yaw,r_wrist_roll,r_wrist_pitch,r_thumb_add,r_thumb_oc,r_index_add,r_index_oc,r_middle_oc,r_ring_pinky_oc,torso_roll,torso_pitch,torso_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>
-->
<!-- These are the parameters with torso_pitch removed -->
<param name="axesNames">(neck_pitch,neck_roll,neck_yaw,camera_tilt,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_yaw,l_wrist_roll,l_wrist_pitch,l_thumb_add,l_thumb_oc,l_index_add,l_index_oc,l_middle_oc,l_ring_pinky_oc,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_yaw,r_wrist_roll,r_wrist_pitch,r_thumb_add,r_thumb_oc,r_index_add,r_index_oc,r_middle_oc,r_ring_pinky_oc,torso_roll,torso_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="joints"> 44 </param>

<!-- <!-\- These are the parameters with torso_pitch removed -\-> -->
<!-- <param name="axesNames">(neck_pitch,neck_roll,neck_yaw,camera_tilt,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_yaw,l_wrist_roll,l_wrist_pitch,l_thumb_add,l_thumb_oc,l_index_add,l_index_oc,l_middle_oc,l_ring_pinky_oc,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_yaw,r_wrist_roll,r_wrist_pitch,r_thumb_add,r_thumb_oc,r_index_add,r_index_oc,r_middle_oc,r_ring_pinky_oc,torso_roll,torso_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="joints"> 45 </param>
<action phase="startup" level="6" type="attach">
<paramlist name="networks">
<elem name="head_joints"> head-mc_remapper </elem>
Expand All @@ -20,4 +21,4 @@
</paramlist>
</action>
<action phase="shutdown" level="19" type="detach" />
</device>
</device>