Skip to content

Commit

Permalink
allow pr2.urdf.xacro to accept customized calibration values (#285)
Browse files Browse the repository at this point in the history 
If 0.0, use the default values. (This retains backward compatibility)
If not 0.0, use the passed values. (Enable custom calibration values for all macro instantiations

Using 0.0 to trigger the old defaults is necessary because some default values need to be computed from other parameters.
  • Loading branch information
Shuang Li committed Oct 5, 2020
1 parent e64f036 commit 60d53a2
Show file tree
Hide file tree
Showing 6 changed files with 102 additions and 29 deletions.
23 changes: 19 additions & 4 deletions pr2_description/urdf/forearm_v0/forearm.urdf.xacro
View file
Expand Up @@ -11,8 +11,9 @@

<!-- ============================ Forearm ============================ -->

<xacro:macro name="pr2_forearm_v0" params="side parent reflect *origin">

<xacro:macro name="pr2_forearm_v0" params="side parent reflect *origin
wrist_flex_calib_ref:=0.0
wrist_roll_calib_ref:=0.0">
<joint name="${side}_forearm_joint" type="fixed">
<xacro:insert_block name="origin" /> <!-- transform from parent link to this joint frame -->
<parent link="${parent}"/>
Expand Down Expand Up @@ -51,7 +52,14 @@
<limit upper="0.0" lower="-2.18" effort="10" velocity="${VELOCITY_LIMIT_SCALE*5.13}" /> <!-- alpha tested velocity and effort limits -->
<safety_controller k_position="20" k_velocity="4" soft_upper_limit="${0.0 - 0.10}" soft_lower_limit="${-2.18 + 0.18}" />
<dynamics damping="0.1" />
<calibration falling="${-0.5410521 + cal_r_wrist_flex_flag}" />

<xacro:unless value="${wrist_flex_calib_ref}">
<calibration falling="${-0.5410521+cal_r_wrist_flex_flag}" />
</xacro:unless>
<xacro:if value="${wrist_flex_calib_ref}">
<calibration falling="${wrist_flex_calib_ref}" />
</xacro:if>

<origin xyz="0.321 0 0" rpy="0 0 0" />
<parent link="${side}_forearm_link"/>
<child link="${side}_wrist_flex_link"/>
Expand Down Expand Up @@ -89,7 +97,14 @@
<limit effort="10" velocity="${VELOCITY_LIMIT_SCALE*6}" /> <!-- alpha tested velocity and effort limits -->
<safety_controller k_velocity="2" />
<dynamics damping="0.1" />
<calibration rising="${-M_PI / 2 + cal_r_wrist_roll_flag}" />

<xacro:unless value="${wrist_roll_calib_ref}">
<calibration rising="${-M_PI/2+cal_r_wrist_roll_flag}" />
</xacro:unless>
<xacro:if value="${wrist_roll_calib_ref}">
<calibration rising="${wrist_roll_calib_ref}" />
</xacro:if>

<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="${side}_wrist_flex_link"/>
<child link="${side}_wrist_roll_link"/>
Expand Down
23 changes: 16 additions & 7 deletions pr2_description/urdf/head_v0/head.urdf.xacro
View file
Expand Up @@ -7,12 +7,12 @@

<xacro:property name="M_PI" value="3.1415926535897931" />

<xacro:macro name="pr2_head_pan_v0" params="name parent *origin">
<xacro:macro name="pr2_head_pan_v0" params="name parent *origin head_pan_calib_ref:=0.0">
<joint name="${name}_joint" type="revolute">
<axis xyz="0 0 1" />
<limit lower="-3.007" upper="3.007" effort="2.645" velocity="6" /> <!-- alpha tested velocity and effort limits -->
<safety_controller k_velocity="1.5" soft_lower_limit="${-3.007+0.15}" soft_upper_limit="${3.007-0.15}" k_position="100" />
<calibration rising="0.0" />
<calibration rising="${head_pan_calib_ref}" />
<dynamics damping="0.5" />
<xacro:insert_block name="origin" />
<parent link="${parent}"/>
Expand Down Expand Up @@ -49,13 +49,20 @@



<xacro:macro name="pr2_head_tilt_v0" params="name parent *origin">
<xacro:macro name="pr2_head_tilt_v0" params="name parent *origin head_tilt_calib_ref:=0.0">

<joint name="${name}_joint" type="revolute">
<axis xyz="0 1 0" />
<limit lower="-0.471238" upper="1.39626" effort="18" velocity="5" /> <!-- alpha tested velocity and effort limits -->
<safety_controller k_velocity="3.0" soft_lower_limit="${-0.4712 + 0.1}" soft_upper_limit="${1.39626 - 0.1}" k_position="100" />
<calibration falling="${0.0+cal_head_tilt_flag}" />

<xacro:unless value="${head_tilt_calib_ref}">
<calibration falling="${0.0+cal_head_tilt_flag}" />
</xacro:unless>
<xacro:if value="${head_tilt_calib_ref}">
<calibration falling="${head_tilt_calib_ref}" />
</xacro:if>

<dynamics damping="10.0" />
<xacro:insert_block name="origin" />
<parent link="${parent}"/>
Expand Down Expand Up @@ -93,12 +100,14 @@



<xacro:macro name="pr2_head_v0" params="name parent *origin">
<xacro:pr2_head_pan_v0 name="${name}_pan" parent="${parent}">
<xacro:macro name="pr2_head_v0" params="name parent *origin
head_pan_calib_ref:=0.0
head_tilt_calib_ref:=0.0 ">
<xacro:pr2_head_pan_v0 name="${name}_pan" parent="${parent}" head_pan_calib_ref="${head_pan_calib_ref}">
<xacro:insert_block name="origin" />
</xacro:pr2_head_pan_v0>

<xacro:pr2_head_tilt_v0 name="${name}_tilt" parent="${name}_pan_link">
<xacro:pr2_head_tilt_v0 name="${name}_tilt" parent="${name}_pan_link" head_tilt_calib_ref="${head_tilt_calib_ref}">
<origin xyz="0.068 0 0" rpy="0 0 0" />
</xacro:pr2_head_tilt_v0>

Expand Down
38 changes: 30 additions & 8 deletions pr2_description/urdf/shoulder_v0/shoulder.urdf.xacro
View file
Expand Up @@ -14,7 +14,10 @@

<!-- ============================ Shoulder ============================ -->

<xacro:macro name="pr2_shoulder_v0" params="side parent reflect *origin">
<xacro:macro name="pr2_shoulder_v0" params="side parent reflect *origin
shoulder_pan_calib_ref:=0.0
shoulder_lift_calib_ref:=0.0
upper_arm_roll_calib_ref:=0.0">

<!-- Shoulder pan -->
<joint name="${side}_shoulder_pan_joint" type="revolute">
Expand All @@ -32,8 +35,12 @@
soft_upper_limit="${reflect*M_PI/4+1.5-0.15}" />

<!-- joint angle when the rising or the falling flag is activated on PR2 -->
<calibration rising="${(reflect*M_PI/4)*cal_r_shoulder_pan_gearing+cal_r_shoulder_pan_flag}" />

<xacro:unless value="${shoulder_pan_calib_ref}">
<calibration rising="${(reflect*M_PI/4)*cal_r_shoulder_pan_gearing+cal_r_shoulder_pan_flag}" />
</xacro:unless>
<xacro:if value="${shoulder_pan_calib_ref}">
<calibration rising="${shoulder_pan_calib_ref}" />
</xacro:if>
</joint>
<link name="${side}_shoulder_pan_link">

Expand Down Expand Up @@ -72,7 +79,13 @@

<safety_controller k_position="100" k_velocity="10"
soft_lower_limit="${-0.5236+0.17}" soft_upper_limit="${1.3963-0.10}" />
<calibration falling="${cal_r_shoulder_lift_flag}" />
<xacro:unless value="${shoulder_lift_calib_ref}">
<calibration falling="${cal_r_shoulder_lift_flag}" />
</xacro:unless>
<xacro:if value="${shoulder_lift_calib_ref}">
<calibration falling="${shoulder_lift_calib_ref}" />
</xacro:if>

<dynamics damping="10.0" />
<origin xyz="0.1 0 0" rpy="0 0 0" />
<parent link="${side}_shoulder_pan_link"/>
Expand Down Expand Up @@ -105,7 +118,8 @@
</link>

<!-- Upper arm roll is separate macro -->
<xacro:pr2_upper_arm_roll_v0 side="${side}" parent="${side}_shoulder_lift_link" reflect="${reflect}" >
<xacro:pr2_upper_arm_roll_v0 side="${side}" parent="${side}_shoulder_lift_link" reflect="${reflect}"
upper_arm_roll_calib_ref="${upper_arm_roll_calib_ref}">
<origin xyz="0 0 0" rpy="0 0 0" />
</xacro:pr2_upper_arm_roll_v0>

Expand All @@ -116,15 +130,23 @@
</xacro:macro>

<!-- Upperarm roll: internal fixed attchment point for upper arm -->
<xacro:macro name="pr2_upper_arm_roll_v0" params="side parent reflect *origin">
<joint name="${side}_upper_arm_roll_joint" type="revolute">
<xacro:macro name="pr2_upper_arm_roll_v0" params="side parent reflect *origin
upper_arm_roll_calib_ref:=0.0">
<joint name="${side}_upper_arm_roll_joint" type="revolute" >
<axis xyz="1 0 0" />
<xacro:insert_block name="origin" />
<parent link="${parent}" />
<child link="${side}_upper_arm_roll_link"/>
<limit lower="${reflect*1.55-2.35}" upper="${reflect*1.55+2.35}" effort="30" velocity="${VELOCITY_LIMIT_SCALE*5.45}" /> <!-- alpha tested velocity and effort limits -->
<safety_controller k_position="100" k_velocity="2" soft_lower_limit="${reflect*1.55-2.35+0.15}" soft_upper_limit="${reflect*1.55+2.35-0.15}" />
<calibration rising="${(reflect*M_PI/2)*cal_r_upper_arm_roll_gearing + cal_r_upper_arm_roll_flag}" />

<xacro:unless value="${upper_arm_roll_calib_ref}">
<calibration rising="${(reflect*M_PI/2)*cal_r_upper_arm_roll_gearing+cal_r_upper_arm_roll_flag}" />
</xacro:unless>
<xacro:if value="${upper_arm_roll_calib_ref}">
<calibration rising="${upper_arm_roll_calib_ref}" />
</xacro:if>

<dynamics damping="0.1" />
</joint>
<link name="${side}_upper_arm_roll_link">
Expand Down
2 changes: 1 addition & 1 deletion pr2_description/urdf/tilting_laser_v0/tilting_laser.urdf.xacro
View file
Expand Up @@ -6,7 +6,7 @@
<xacro:include filename="$(find pr2_description)/urdf/tilting_laser_v0/tilting_laser.transmission.xacro" />
<xacro:include filename="$(find pr2_description)/urdf/sensors/hokuyo_lx30_laser.urdf.xacro" />

<xacro:macro name="pr2_tilting_laser_v0" params="name parent laser_calib_ref *origin">
<xacro:macro name="pr2_tilting_laser_v0" params="name parent laser_calib_ref:=0.0 *origin">

<joint name="${name}_mount_joint" type="revolute">
<axis xyz="0 1 0" />
Expand Down
11 changes: 9 additions & 2 deletions pr2_description/urdf/torso_v0/torso.urdf.xacro
View file
Expand Up @@ -5,12 +5,19 @@
<xacro:include filename="$(find pr2_description)/urdf/torso_v0/torso.transmission.xacro" />
<xacro:include filename="$(find pr2_description)/urdf/sensors/microstrain_3dmgx2_imu.urdf.xacro" />

<xacro:macro name="pr2_torso_v0" params="name parent *origin">
<xacro:macro name="pr2_torso_v0" params="name parent *origin torso_calib_ref:=0.0">
<joint name="${name}_joint" type="prismatic">
<axis xyz="0 0 1" />
<limit lower="0.0" upper="0.33" effort="10000" velocity="0.013"/> <!-- alpha tested velocity and effort limits -->
<safety_controller k_velocity="2000000" soft_lower_limit="${0.0+0.0115}" soft_upper_limit="${0.33-0.005}" k_position="100" />
<calibration falling="0.00475" />

<xacro:unless value="${torso_calib_ref}">
<calibration falling="0.00475" />
</xacro:unless>
<xacro:if value="${torso_calib_ref}">
<calibration falling="${torso_calib_ref}" />
</xacro:if>

<dynamics damping="20000.0" />
<xacro:insert_block name="origin" />
<parent link="${parent}"/>
Expand Down
34 changes: 27 additions & 7 deletions pr2_description/urdf/upper_arm_v0/upper_arm.urdf.xacro
View file
Expand Up @@ -33,7 +33,9 @@

<!-- ============================ Upper Arm ============================ -->
<!-- Includes elbow flex, FA roll joints in macros below -->
<xacro:macro name="pr2_upper_arm_v0" params="side parent reflect">
<xacro:macro name="pr2_upper_arm_v0" params="side parent reflect
forearm_roll_calib_ref:=0.0
elbow_flex_calib_ref:=0.0 ">
<joint name="${side}_upper_arm_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="${parent}"/>
Expand Down Expand Up @@ -76,18 +78,28 @@
<!-- No transmission, since this a fixed joint w/o actuator -->

<!-- Elbow flex, FA roll macros -->
<xacro:pr2_forearm_roll_v0 side="${side}" parent="${side}_elbow_flex_link" reflect="${reflect}" />
<xacro:pr2_elbow_flex_v0 side="${side}" parent="${side}_upper_arm_link" reflect="${reflect}" />
<xacro:pr2_forearm_roll_v0 side="${side}" parent="${side}_elbow_flex_link" reflect="${reflect}"
forearm_roll_calib_ref="${forearm_roll_calib_ref}"/>
<xacro:pr2_elbow_flex_v0 side="${side}" parent="${side}_upper_arm_link" reflect="${reflect}"
elbow_flex_calib_ref="${elbow_flex_calib_ref}"/>
</xacro:macro>

<xacro:macro name="pr2_elbow_flex_v0" params="side parent reflect">
<xacro:macro name="pr2_elbow_flex_v0" params="side parent reflect
elbow_flex_calib_ref:=0.0">
<!-- Elbow flex -->
<joint name="${side}_elbow_flex_joint" type="revolute">
<axis xyz="0 1 0" />
<!-- Note: Overtravel limits are 140, -7 degrees instead of 133, 0 -->
<limit lower="-2.3213" upper="0.00" effort="30" velocity="${VELOCITY_LIMIT_SCALE*5.5}" /> <!-- alpha tested velocity and effort limits -->
<safety_controller k_position="100" k_velocity="3" soft_lower_limit="${-2.3213+0.2}" soft_upper_limit="${0.00-0.15}" />
<calibration falling="${-1.1606 + cal_r_elbow_flex_flag}"/>

<xacro:unless value="${elbow_flex_calib_ref}">
<calibration falling="${-1.1606+cal_r_elbow_flex_flag}"/>
</xacro:unless>
<xacro:if value="${elbow_flex_calib_ref}">
<calibration falling="${elbow_flex_calib_ref}"/>
</xacro:if>

<dynamics damping="1.0" />
<origin xyz="0.4 0 0" rpy="0 0 0" />
<parent link="${parent}"/>
Expand Down Expand Up @@ -124,13 +136,21 @@
</xacro:macro>

<!-- FA roll joint only -->
<xacro:macro name="pr2_forearm_roll_v0" params="side parent reflect">
<xacro:macro name="pr2_forearm_roll_v0" params="side parent reflect
forearm_roll_calib_ref:=0.0">
<!-- forearm_roll_link is a fictitious link internal to elbow_flex_link, provides an attachment point for the actual forearm -->
<joint name="${side}_forearm_roll_joint" type="continuous">
<axis xyz="1 0 0" />
<limit effort="30" velocity="${VELOCITY_LIMIT_SCALE*6}" /> <!-- alpha tested velocity and effort limits -->
<safety_controller k_velocity="1" />
<calibration rising="${0.0 + cal_r_forearm_roll_flag}" />

<xacro:unless value="${forearm_roll_calib_ref}">
<calibration rising="${0.0+cal_r_forearm_roll_flag}" />
</xacro:unless>
<xacro:if value="${forearm_roll_calib_ref}">
<calibration rising="${forearm_roll_calib_ref}" />
</xacro:if>

<dynamics damping="0.1" />
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="${parent}"/>
Expand Down

0 comments on commit 60d53a2

Please sign in to comment.