Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[8245] pkg/trunk
|
From: <hsu...@us...> - 2008-12-17 03:21:16
|
Revision: 8245
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8245&view=rev
Author: hsujohnhsu
Date: 2008-12-17 03:21:11 +0000 (Wed, 17 Dec 2008)
Log Message:
-----------
added gazebo dynamics controller for arm.
Added Paths:
-----------
pkg/trunk/demos/arm_gazebo/arm_grav.launch
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_grav_arm.xacro.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_grav_defs.xml
Added: pkg/trunk/demos/arm_gazebo/arm_grav.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/arm_grav.launch (rev 0)
+++ pkg/trunk/demos/arm_gazebo/arm_grav.launch 2008-12-17 03:21:11 UTC (rev 8245)
@@ -0,0 +1,28 @@
+<launch>
+ <group name="wg">
+ <!-- send pr2_arm.xml to param server -->
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2_arm_test/pr2_grav_arm.xacro.xml'" />
+
+ <!-- -g flag runs gazebo in gui-less mode -->
+ <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(find boost)/boost/lib" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ </node>
+
+ <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen"/>
+
+ <!-- start arm controller -->
+ <!--node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/l_arm_default_controller.xml" respawn="false" /--> <!-- load default arm controller -->
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/l_arm_dynamic_controller.xml" respawn="false" /> <!-- load default arm controller -->
+
+ <!-- send arm a command -->
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set l_gripper_controller 0.5" respawn="false" output="screen" /> <!-- open gripper .5 radians -->
+
+ <!-- for visualization -->
+ <!-- node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" / -->
+ </group>
+</launch>
+
Added: pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_grav_arm.xacro.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_grav_arm.xacro.xml (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_grav_arm.xacro.xml 2008-12-17 03:21:11 UTC (rev 8245)
@@ -0,0 +1,77 @@
+<?xml version="1.0"?>
+<robot name="pr2"
+ xmlns:xi="http://www.w3.org/2001/XInclude"
+ xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
+ xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
+ xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+ xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
+ xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
+ xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
+ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+ xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
+ xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
+ xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+ xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics">
+
+
+ <!-- declare the path where all models/textures/materials are stored -->
+ <resource location="ros-pkg://wg_robot_description/models/pr2"/>
+
+ <include filename="../pr2_robot_defs/arm_grav_defs.xml" />
+ <include filename="../pr2_robot_defs/gripper_defs.xml" />
+ <include filename="../pr2_robot_defs/gazebo_defs.xml" />
+ <include filename="./groups_arm.xml" />
+
+ <pr2_arm side="l" reflect="1" parent="base">
+ <origin xyz="0.0 0.0 1.0" rpy="0 0 0" />
+ </pr2_arm>
+ <pr2_gripper side="l" parent="l_wrist_roll" />
+
+ <!-- Solid Base -->
+ <joint name="base_joint" type="planar">
+ </joint>
+ <link name="base_link"><!-- link specifying the base of the robot -->
+ <parent name=" world" />
+ <!-- rotation of a local coordinate frame attached to the link with respect to a global coordinate frame -->
+ <origin xyz=" 0 0 0.002 " rpy=" 0 0 0" /> <!-- position of a local coordinate frame attached to the link with respect to the parent link's coordinate frame -->
+ <joint name="base_joint" />
+ <inertial>
+ <mass value="1000" />
+ <com xyz=" 0 0 0 " /> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
+ <inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0 " /> <!-- location defined with respect to the link origin in a local coordinate frame -->
+ <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/White</elem>
+ </map>
+ <geometry name="pr2_base_mesh_file">
+ <mesh scale="20 20 0.01" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0.0 0.0 0.0 " /> <!-- default box is centered at the origin -->
+ <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <geometry name="base_collision_geom"> <!-- think about putting mesh here as well -->
+ <box size="20 20 0.01" />
+ </geometry>
+ </collision>
+ </link>
+
+ <map name="gazebo_material" flag="gazebo">
+ <verbatim>
+ <!-- P3D for position groundtruth -->
+ <controller:P3D name="p3d_l_wrist_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>l_gripper_palm_link</bodyName>
+ <topicName>l_gripper_palm_pose_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="p3d_l_wrist_position"/>
+ </controller:P3D>
+ </verbatim>
+ </map>
+
+
+</robot>
Added: pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_grav_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_grav_defs.xml (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_grav_defs.xml 2008-12-17 03:21:11 UTC (rev 8245)
@@ -0,0 +1,630 @@
+<?xml version="1.0"?>
+<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+ xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">
+
+ <property name="M_PI" value="3.1415926535897931" />
+ <property name="VELOCITY_LIMIT_SCALE" value="0.6" />
+
+ <property name="shoulder_lift_length" value="0.10" />
+ <property name="shoulder_lift_radius" value="0.12" />
+
+
+
+
+
+
+
+
+ <macro name="pr2_upper_arm" params="side parent reflect *origin">
+
+ <!-- Shoulder pan -->
+
+ <joint name="${side}_shoulder_pan_joint" type="revolute">
+ <axis xyz="0 0 1" />
+
+ <limit min="${reflect*M_PI/4-1.5}" max="${reflect*M_PI/4+1.5}"
+ effort="30" velocity="${VELOCITY_LIMIT_SCALE*3.48}"
+ k_position="100" k_velocity="10"
+ safety_length_min="0.15" safety_length_max="0.15" />
+ <calibration reference_position="${reflect*M_PI/4}" values="1.5 -1" />
+
+ <joint_properties damping="10.0" />
+ </joint>
+
+ <link name="${side}_shoulder_pan_link">
+ <parent name="${parent}_link" />
+ <insert_block name="origin" />
+ <joint name="${side}_shoulder_pan_joint" />
+ <inertial>
+ <mass value="16.29553" />
+ <com xyz="-0.005215 -0.030552 -0.175743" />
+ <inertia ixx="0.793291393007" ixy="0.003412032973" ixz="0.0096614481"
+ iyy="0.818419457224" iyz="-0.033999499551"
+ izz="0.13915007406" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/Blue</elem>
+ </map>
+ <geometry name="${side}_shoulder_pan_visual">
+ <mesh filename="shoulder_yaw" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0.05 0 -0.2" rpy="0 0 0 " />
+ <geometry name="${side}_shoulder_pan_collision">
+ <box size="0.347 0.254 0.646" />
+ </geometry>
+ </collision>
+ <map name="${side}_shoulder_pan_sensor" flag="gazebo">
+ <verbatim key="${side}_shoulder_pan_bumper_sensor">
+ <sensor:contact name="${side}_shoulder_pan_contact_sensor">
+ <geom>${side}_shoulder_pan_collision</geom>
+ <updateRate>1000.0</updateRate>
+ <controller:ros_bumper name="${side}_shoulder_pan_ros_bumper_controller" plugin="libRos_Bumper.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bumperTopicName>${side}_shoulder_pan_bumper</bumperTopicName>
+ <interface:bumper name="${side}_shoulder_pan_ros_bumper_iface" />
+ </controller:ros_bumper>
+ </sensor:contact>
+ </verbatim>
+ </map>
+ <map name="${side}_shoulder_pan_gravity" flag="gazebo">
+ <elem key="turnGravityOff">false</elem>
+ </map>
+ </link>
+
+ <transmission type="SimpleTransmission" name="${side}_shoulder_pan_trans">
+ <actuator name="${side}_shoulder_pan_motor" />
+ <joint name="${side}_shoulder_pan_joint" />
+ <mechanicalReduction>63.16</mechanicalReduction>
+ </transmission>
+
+ <!-- Shoulder lift -->
+
+ <joint name="${side}_shoulder_lift_joint" type="revolute">
+ <axis xyz="0 1 0" />
+ <anchor xyz="0 0 0" />
+
+ <limit min="-0.4" max="1.5" effort="30" velocity="${VELOCITY_LIMIT_SCALE*3.47}"
+ k_position="100" k_velocity="10"
+ safety_length_min="0.1" safety_length_max="0.1" />
+ <calibration reference_position="0" values="1.5 -1 " />
+
+ <joint_properties damping="100.0" />
+ </joint>
+
+ <link name="${side}_shoulder_lift_link">
+ <parent name="${side}_shoulder_pan_link" />
+ <origin xyz="0.1 0 0" rpy="0 0 0" />
+ <joint name="${side}_shoulder_lift_joint" />
+ <inertial>
+ <mass value="2.41223" />
+ <com xyz="-0.035895 0.014422 -0.028263" />
+ <inertia ixx="0.016640333585" ixy="0.002696462583" ixz="0.001337742275"
+ iyy="0.017232603914" iyz="-0.003605106514" izz="0.018723553425" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/Grey</elem>
+ </map>
+ <geometry name="${side}_shoulder_lift_visual">
+ <mesh filename="shoulder_lift" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0 " rpy="${M_PI/2} 0 0" />
+ <geometry name="${side}_shoulder_lift_collision">
+ <cylinder radius="${shoulder_lift_radius}" length="${shoulder_lift_length}" />
+ </geometry>
+ </collision>
+ <map name="${side}_shoulder_lift_sensor" flag="gazebo">
+ <verbatim key="${side}_shoulder_lift_bumper_sensor">
+ <sensor:contact name="${side}_shoulder_lift_contact_sensor">
+ <geom>${side}_shoulder_lift_collision</geom>
+ <updateRate>1000.0</updateRate>
+ <controller:ros_bumper name="${side}_shoulder_lift_ros_bumper_controller" plugin="libRos_Bumper.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bumperTopicName>${side}_shoulder_lift_bumper</bumperTopicName>
+ <interface:bumper name="${side}_shoulder_lift_ros_bumper_iface" />
+ </controller:ros_bumper>
+ </sensor:contact>
+ </verbatim>
+ </map>
+ <map name="${side}_shoulder_lift_gravity" flag="gazebo">
+ <elem key="turnGravityOff">false</elem>
+ </map>
+ </link>
+
+ <transmission type="SimpleTransmission" name="${side}_shoulder_lift_trans">
+ <actuator name="${side}_shoulder_lift_motor" />
+ <joint name="${side}_shoulder_lift_joint" />
+ <mechanicalReduction>57.36</mechanicalReduction>
+ </transmission>
+
+ <!-- Upperarm roll -->
+
+ <joint name="${side}_upper_arm_roll_joint" type="revolute">
+ <axis xyz="1 0 0" />
+ <anchor xyz="0 0 0" />
+
+ <limit min="-3.9" max="0.8" effort="30" velocity="${VELOCITY_LIMIT_SCALE*5.45}"
+ k_position="100" k_velocity="2"
+ safety_length_min="0.15" safety_length_max="0.15" />
+ <calibration reference_position="${-M_PI/2}" values="1.5 -1 " />
+
+ <joint_properties damping="1.0" />
+ </joint>
+
+ <link name="${side}_upper_arm_roll_link">
+ <parent name="${side}_shoulder_lift_link" />
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <joint name="${side}_upper_arm_roll_joint" />
+
+ <inertial>
+ <mass value="4.9481" />
+ <com xyz=" 0.21227 0.001205 -0.016293 " /> <!-- x becomes y, y becomes z, z becomes x, not sure about the signs -->
+ <inertia ixx="0.01338328491" ixy="0.00011947689" ixz="0.00154493231"
+ iyy="0.07306071531" iyz="0.00054774592"
+ izz="0.07212451075" /> <!-- x becomes y, y becomes z, z becomes x, not sure about the signs -->
+ </inertial>
+
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/Green</elem>
+ </map>
+ <geometry name="${side}_upper_arm_roll_visual">
+ <mesh filename="upperarm_roll" />
+ </geometry>
+ </visual>
+
+ <collision>
+ <origin xyz="0.3 0 0" rpy="0 0 0" />
+ <geometry name="${side}_upper_arm_roll_collision">
+ <box size="0.362 0.144 0.157" />
+ </geometry>
+ </collision>
+ <map name="${side}_upper_arm_roll_sensor" flag="gazebo">
+ <verbatim key="${side}_upper_arm_roll_bumper_sensor">
+ <sensor:contact name="${side}_upper_arm_roll_contact_sensor">
+ <geom>${side}_upper_arm_roll_collision</geom>
+ <updateRate>1000.0</updateRate>
+ <controller:ros_bumper name="${side}_upper_arm_roll_ros_bumper_controller" plugin="libRos_Bumper.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bumperTopicName>${side}_upper_arm_roll_bumper</bumperTopicName>
+ <interface:bumper name="${side}_upper_arm_roll_ros_bumper_iface" />
+ </controller:ros_bumper>
+ </sensor:contact>
+ </verbatim>
+ </map>
+
+ <map name="${side}_upper_arm_roll_gravity" flag="gazebo">
+ <elem key="turnGravityOff">false</elem>
+ </map>
+ </link>
+
+ <transmission type="SimpleTransmission" name="${side}_upper_arm_roll_trans">
+ <actuator name="${side}_upper_arm_roll_motor" />
+ <joint name="${side}_upper_arm_roll_joint" />
+ <mechanicalReduction>32.65</mechanicalReduction>
+ </transmission>
+
+ <!-- Elbow flex -->
+
+ <joint name="${side}_elbow_flex_joint" type="revolute">
+ <axis xyz="0 -1 0" />
+ <anchor xyz="0 0 0" />
+
+ <limit min="-0.1" max="2.3" effort="30" velocity="${VELOCITY_LIMIT_SCALE*5.5}"
+ k_position="100" k_velocity="3"
+ safety_length_min="0.1" safety_length_max="0.4" />
+ <calibration reference_position="1.1" values="1.5 -1" />
+
+ <joint_properties damping="10.0" />
+ </joint>
+
+ <link name="${side}_elbow_flex_link">
+ <parent name="${side}_upper_arm_roll_link" />
+ <origin xyz="0.4 0 0" rpy="0 0 0" />
+ <joint name="${side}_elbow_flex_joint" />
+
+ <inertial>
+ <mass value="1.689246" />
+ <com xyz="0.013173 0.001228 -0.011638" /> <!-- switching y to x, z to y, x to z -->
+ <inertia ixx="0.00323681521" ixy="-0.00000116216" ixz="0.00029204667"
+ iyy="0.00410053774" iyz="-0.00007735928"
+ izz="0.00327529855" /> <!-- switching y to x, z to y, x to z -->
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/Grey</elem>
+ </map>
+ <geometry name="${side}_elbow_flex_visual">
+ <mesh filename="elbow_flex" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
+ <geometry name="${side}_elbow_flex_collision">
+ <cylinder radius="0.1" length="0.08" />
+ </geometry>
+ </collision>
+ <map name="${side}_elbow_flex_sensor" flag="gazebo">
+ <verbatim key="${side}_elbow_flex_bumper_sensor">
+ <sensor:contact name="${side}_elbow_flex_contact_sensor">
+ <geom>${side}_elbow_flex_collision</geom>
+ <updateRate>1000.0</updateRate>
+ <controller:ros_bumper name="${side}_elbow_flex_ros_bumper_controller" plugin="libRos_Bumper.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bumperTopicName>${side}_elbow_flex_bumper</bumperTopicName>
+ <interface:bumper name="${side}_elbow_flex_ros_bumper_iface" />
+ </controller:ros_bumper>
+ </sensor:contact>
+ </verbatim>
+ </map>
+ <map name="${side}_elbow_flex_gravity" flag="gazebo">
+ <elem key="turnGravityOff">false</elem>
+ </map>
+ </link>
+
+ <transmission type="SimpleTransmission" name="${side}_elbow_flex_trans">
+ <actuator name="${side}_elbow_flex_motor" />
+ <joint name="${side}_elbow_flex_joint" />
+ <mechanicalReduction>36.17</mechanicalReduction>
+ </transmission>
+
+ <map name="sensor" flag="gazebo">
+ <verbatim key="p3d_${side}_upper_arm">
+ <!--
+ <controller:P3D name="p3d_${side}_shoulder_pan_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>${side}_shoulder_pan_link</bodyName>
+ <topicName>${side}_shoulder_pan_pose_ground_truth</topicName>
+ <gaussianNoise>0.0</gaussianNoise>
+ <frameName>map</frameName>
+ <interface:position name="p3d_${side}_shoulder_pan_position_iface" />
+ </controller:P3D>
+ <controller:P3D name="p3d_${side}_shoulder_lift_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>${side}_shoulder_lift_link</bodyName>
+ <topicName>${side}_shoulder_lift_pose_ground_truth</topicName>
+ <gaussianNoise>0.0</gaussianNoise>
+ <frameName>map</frameName>
+ <interface:position name="p3d_${side}_shoulder_lift_position_iface" />
+ </controller:P3D>
+ <controller:P3D name="p3d_${side}_upper_arm_roll_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>${side}_upper_arm_roll_link</bodyName>
+ <topicName>${side}_upper_arm_roll_pose_ground_truth</topicName>
+ <gaussianNoise>0.0</gaussianNoise>
+ <frameName>map</frameName>
+ <interface:position name="p3d_${side}_upper_arm_roll_position_iface" />
+ </controller:P3D>
+ <controller:P3D name="p3d_${side}_elbow_flex_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>${side}_elbow_flex_link</bodyName>
+ <topicName>${side}_elbow_flex_pose_ground_truth</topicName>
+ <gaussianNoise>0.0</gaussianNoise>
+ <frameName>map</frameName>
+ <interface:position name="p3d_${side}_elbow_flex_position_iface" />
+ </controller:P3D>
+ -->
+ </verbatim>
+ </map>
+
+
+ </macro>
+
+
+
+ <macro name="pr2_forearm" params="side parent reflect">
+
+ <!-- Forearm roll -->
+
+ <joint name="${side}_forearm_roll_joint" type="revolute">
+ <axis xyz="-1 0 0" />
+ <anchor xyz="0 0 0" />
+
+ <limit effort="30" velocity="${VELOCITY_LIMIT_SCALE*6}" k_velocity="1" />
+ <calibration reference_position="0" values="1.5 -1" />
+
+ <joint_properties damping="1.0" />
+ </joint>
+
+ <link name="${side}_forearm_roll_link">
+ <parent name="${parent}_link" />
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <joint name="${side}_forearm_roll_joint" />
+ <inertial>
+ <mass value="1.804155" />
+ <com xyz="0.179474 -0.000058 0.013779" /> <!-- z to x, x to y, y to z -->
+ <inertia ixx="0.00175508987" ixy="0.00002937939" ixz="-0.00042767904"
+ iyy="0.01243055254" iyz="-0.00000367110"
+ izz="0.01356754885" /> <!-- z to x, x to y, y to z -->
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/Blue</elem>
+ </map>
+ <geometry name="${side}_forearm_roll_visual">
+ <mesh filename="forearm_roll" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0.22 0 0" rpy="0 0 0" />
+ <geometry name="${side}_forearm_roll_collision">
+ <box size="0.27 0.12 0.08" />
+ </geometry>
+ </collision>
+ <map name="${side}_forearm_roll_sensor" flag="gazebo">
+ <verbatim key="${side}_forearm_roll_bumper_sensor">
+ <sensor:contact name="${side}_forearm_roll_contact_sensor">
+ <geom>${side}_forearm_roll_collision</geom>
+ <updateRate>1000.0</updateRate>
+ <controller:ros_bumper name="${side}_forearm_roll_ros_bumper_controller" plugin="libRos_Bumper.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bumperTopicName>${side}_forearm_roll_bumper</bumperTopicName>
+ <interface:bumper name="${side}_forearm_roll_ros_bumper_iface" />
+ </controller:ros_bumper>
+ </sensor:contact>
+ </verbatim>
+ </map>
+ <map name="${side}_forearm_roll_gravity" flag="gazebo">
+ <elem key="turnGravityOff">false</elem>
+ </map>
+ </link>
+
+ <transmission type="SimpleTransmission" name="${side}_forearm_roll_trans">
+ <actuator name="${side}_forearm_roll_motor" />
+ <joint name="${side}_forearm_roll_joint" />
+ <mechanicalReduction>${576/25*55/14}</mechanicalReduction>
+ </transmission>
+
+ <!-- Wrist flex -->
+
+ <joint name="${side}_wrist_flex_joint" type="revolute">
+ <axis xyz="0 -1 0" />
+ <anchor xyz="0 0 0" />
+
+ <limit min="-0.1" max="2.2" effort="200" velocity="${VELOCITY_LIMIT_SCALE*5.13}"
+ k_position="20" k_velocity="4"
+ safety_length_min="0.2" safety_length_max="0.2" />
+ <calibration reference_position="0.4" values="1.5 -1" />
+
+ <joint_properties damping="1.0" />
+ </joint>
+
+ <link name="${side}_wrist_flex_link">
+ <parent name="${side}_forearm_roll_link" />
+ <origin xyz="0.32025 0 0" rpy="0 0 0" />
+ <joint name="${side}_wrist_flex_joint" />
+ <inertial>
+ <mass value="0.80305" />
+ <com xyz="0.012193 0.000017 -0.004929" /> <!-- my best guess z to z, y to x, x to y -->
+ <inertia ixx="0.00077829551" ixy="0.00018491414" ixz="-0.00000036897"
+ iyy="0.00108384500" iyz="0.00000017027"
+ izz="0.00071460255" /> <!-- my best guess z to z, y to x, x to y -->
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/Grey</elem>
+ </map>
+ <geometry name="${side}_wrist_flex_visual">
+ <mesh filename="wrist_flex" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
+ <geometry name="${side}_wrist_flex_collision">
+ <cylinder radius="0.033" length="0.103" />
+ </geometry>
+ </collision>
+ <map name="${side}_wrist_flex_sensor" flag="gazebo">
+ <verbatim key="${side}_wrist_flex_bumper_sensor">
+ <sensor:contact name="${side}_wrist_flex_contact_sensor">
+ <geom>${side}_wrist_flex_collision</geom>
+ <updateRate>1000.0</updateRate>
+ <controller:ros_bumper name="${side}_wrist_flex_ros_bumper_controller" plugin="libRos_Bumper.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bumperTopicName>${side}_wrist_flex_bumper</bumperTopicName>
+ <interface:bumper name="${side}_wrist_flex_ros_bumper_iface" />
+ </controller:ros_bumper>
+ </sensor:contact>
+ </verbatim>
+ </map>
+ <map name="${side}_wrist_flex_gravity" flag="gazebo">
+ <elem key="turnGravityOff">false</elem>
+ </map>
+ </link>
+
+ <!-- Wrist roll -->
+
+ <joint name="${side}_wrist_roll_joint" type="revolute">
+ <axis xyz="1 0 0" />
+ <anchor xyz="0 0 0" />
+ <limit effort="10" velocity="${VELOCITY_LIMIT_SCALE*6}" k_velocity="2" />
+ <calibration reference_position="1.27" values="1.5 -1" />
+ <joint_properties damping="0.1" />
+ </joint>
+
+
+ <link name="${side}_wrist_roll_link">
+ <parent name="${side}_wrist_flex_link" />
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <joint name="${side}_wrist_roll_joint" />
+ <inertial>
+ <mass value="0.1" />
+ <com xyz="0 0 0" />
+ <inertia ixx="0.01" ixy="0" ixz="0"
+ iyy="0.01" iyz="0"
+ izz="0.01" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/Red</elem>
+ </map>
+ <geometry name="${side}_wrist_roll_visual">
+ <mesh filename="wr_roll" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0.0 0 0" rpy="0 0 0" />
+ <geometry name="${side}_wrist_roll_collision">
+ <box size="0.01 0.01 0.01" />
+ </geometry>
+ <verbose value="Yes" />
+ </collision>
+ <map name="${side}_wrist_roll_sensor" flag="gazebo">
+ <verbatim key="${side}_wrist_roll_bumper_sensor">
+ <sensor:contact name="${side}_wrist_roll_contact_sensor">
+ <geom>${side}_wrist_roll_collision</geom>
+ <updateRate>1000.0</updateRate>
+ <controller:ros_bumper name="${side}_wrist_roll_ros_bumper_controller" plugin="libRos_Bumper.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bumperTopicName>${side}_wrist_roll_bumper</bumperTopicName>
+ <interface:bumper name="${side}_wrist_roll_ros_bumper_iface" />
+ </controller:ros_bumper>
+ </sensor:contact>
+ </verbatim>
+ </map>
+ <map name="${side}_wrist_roll_gravity" flag="gazebo">
+ <elem key="turnGravityOff">false</elem>
+ </map>
+ </link>
+
+ <transmission type="WristTransmission" name="${side}_wrist_trans">
+ <rightActuator name="${side}_wrist_r_motor" />
+ <leftActuator name="${side}_wrist_l_motor" />
+ <flexJoint name="${side}_wrist_flex_joint" mechanicalReduction="${624/35*54/16}" />
+ <rollJoint name="${side}_wrist_roll_joint" mechanicalReduction="${624/35*54/16}" />
+ </transmission>
+
+ <map name="sensor" flag="gazebo">
+ <verbatim key="p3d_${side}_forearm">
+ <!--
+ <controller:P3D name="p3d_${side}_forearm_roll_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>${side}_forearm_roll_link</bodyName>
+ <topicName>${side}_forearm_roll_pose_ground_truth</topicName>
+ <gaussianNoise>0.0</gaussianNoise>
+ <frameName>map</frameName>
+ <interface:position name="p3d_${side}_forearm_roll_position_iface" />
+ </controller:P3D>
+ <controller:P3D name="p3d_${side}_wrist_flex_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>${side}_wrist_flex_link</bodyName>
+ <topicName>${side}_wrist_flex_pose_ground_truth</topicName>
+ <gaussianNoise>0.0</gaussianNoise>
+ <frameName>map</frameName>
+ <interface:position name="p3d_${side}_wrist_flex_position_iface" />
+ </controller:P3D>
+ <controller:P3D name="p3d_${side}_wrist_roll_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>${side}_wrist_roll_link</bodyName>
+ <topicName>${side}_wrist_roll_pose_ground_truth</topicName>
+ <gaussianNoise>0.0</gaussianNoise>
+ <frameName>map</frameName>
+ <interface:position name="p3d_${side}_wrist_roll_position_iface" />
+ </controller:P3D>
+ -->
+ </verbatim>
+ </map>
+
+ </macro>
+
+
+
+ <macro name="pr2_arm" params="side parent reflect *origin">
+ <pr2_upper_arm side="${side}" reflect="${reflect}" parent="${parent}">
+ <insert_block name="origin" />
+ </pr2_upper_arm>
+ <pr2_forearm side="${side}" reflect="${reflect}" parent="${side}_elbow_flex" />
+ </macro>
+
+
+
+ <!-- Calibration -->
+
+
+ <macro name="upper_arm_calibrator" params="side">
+ <controller name="${side}_cal_shoulder_pan" topic="${side}_cal_shoulder_pan"
+ type="JointCalibrationControllerNode">
+ <calibrate joint="${side}_shoulder_pan_joint"
+ actuator="${side}_shoulder_pan_motor"
+ transmission="${side}_shoulder_pan_trans"
+ velocity="1.0" />
+ <pid p="7" i="0.5" d="0" iClamp="1.0" />
+ </controller>
+
+ <controller name="cal_shoulder_lift_${side}" topic="cal_shoulder_lift_${side}"
+ type="JointCalibrationControllerNode">
+ <calibrate joint="${side}_shoulder_lift_joint"
+ actuator="${side}_shoulder_lift_motor"
+ transmission="${side}_shoulder_lift_trans"
+ velocity="-1.0" />
+ <pid p="7" i="0.5" d="0" iClamp="1.0" />
+ </controller>
+
+ <controller name="cal_${side}_upper_arm_roll" topic="cal_${side}_upper_arm_roll"
+ type="JointCalibrationControllerNode">
+ <calibrate joint="${side}_upper_arm_roll_joint"
+ actuator="${side}_upper_arm_roll_motor"
+ transmission="${side}_upper_arm_roll_trans"
+ velocity="1.0" />
+ <pid p="6" i="0.2" d="0" iClamp="2.0" />
+ </controller>
+
+ <controller name="cal_${side}_elbow_flex" topic="cal_${side}_elbow_flex"
+ type="JointCalibrationControllerNode">
+ <calibrate joint="${side}_elbow_flex_joint"
+ actuator="${side}_elbow_flex_motor"
+ transmission="${side}_elbow_flex_trans"
+ velocity="1.0" />
+ <pid p="6" i="0.2" d="0" iClamp="1.0" />
+ </controller>
+ </macro>
+
+
+ <macro name="forearm_calibrator" params="side">
+ <controller name="cal_${side}_forearm_roll" type="JointCalibrationControllerNode">
+ <calibrate joint="${side}_forearm_roll_joint"
+ actuator="${side}_forearm_roll_motor"
+ transmission="${side}_forearm_roll_trans"
+ velocity="-1.2" />
+ <pid p="5.0" i="0" d="0" iClamp="0" />
+ </controller>
+
+ <controller type="WristCalibrationControllerNode" name="cal_wrist">
+ <calibrate transmission="${side}_wrist_trans"
+ actuator_l="${side}_wrist_l_motor" actuator_r="${side}_wrist_r_motor"
+ flex_joint="${side}_wrist_flex_joint" roll_joint="${side}_wrist_roll_joint"
+ velocity="1.2" />
+ <pid p="3.0" i="0.2" d="0" iClamp="2.0" />
+ </controller>
+ </macro>
+
+
+
+</robot>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
