Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[10295] pkg/trunk
|
From: <hsu...@us...> - 2009-01-27 23:30:19
|
Revision: 10295
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10295&view=rev
Author: hsujohnhsu
Date: 2009-01-27 23:30:15 +0000 (Tue, 27 Jan 2009)
Log Message:
-----------
* udpate gripper limit, friction parameters.
* update grasp sim demo.
* added a 10khz world.
* update gripper controller gains in sim.
* update table object friction coefs and mass.
Modified Paths:
--------------
pkg/trunk/demos/arm_gazebo/grasp_preprogrammed.py
pkg/trunk/demos/arm_gazebo/l_arm_default_controller.xml
pkg/trunk/demos/arm_gazebo/r_arm_default_controller.xml
pkg/trunk/demos/examples_gazebo/table_defs/table_defs.xml
pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml
Added Paths:
-----------
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/empty_10000hz.world
Modified: pkg/trunk/demos/arm_gazebo/grasp_preprogrammed.py
===================================================================
--- pkg/trunk/demos/arm_gazebo/grasp_preprogrammed.py 2009-01-27 23:09:00 UTC (rev 10294)
+++ pkg/trunk/demos/arm_gazebo/grasp_preprogrammed.py 2009-01-27 23:30:15 UTC (rev 10295)
@@ -57,9 +57,10 @@
CMD_POS_5 = 0.0
CMD_POS_6 = 0.0
CMD_POS_7 = 0.0
-CMD_POS_8 = 0.5
LOWER_Z = 0.1
PAN_RAD = 0.2
+G_OPEN = 0.5;
+G_CLOSE = 0.25;
p3d_received = False
@@ -81,45 +82,45 @@
#open gripper
pub_r_arm.publish(JointPosCmd(['r_shoulder_pan_joint','r_shoulder_lift_joint','r_upper_arm_roll_joint','r_elbow_flex_joint','r_forearm_roll_joint','r_wrist_flex_joint','r_wrist_roll_joint'],[CMD_POS_1,CMD_POS_2,CMD_POS_3,CMD_POS_4,CMD_POS_5,CMD_POS_6,CMD_POS_7],[0,0,0,0,0,0,0],0))
- pub_r_gripper.publish(Float64(CMD_POS_8))
+ pub_r_gripper.publish(Float64(G_OPEN))
time.sleep(8)
#lower arm
pub_r_arm.publish(JointPosCmd(['r_shoulder_pan_joint','r_shoulder_lift_joint','r_upper_arm_rolr_joint','r_elbow_flex_joint','r_forearm_roll_joint','r_wrist_flex_joint','r_wrist_roll_joint'],[CMD_POS_1,CMD_POS_2+LOWER_Z,CMD_POS_3,CMD_POS_4,CMD_POS_5,CMD_POS_6,CMD_POS_7],[0,0,0,0,0,0,0],0))
- pub_r_gripper.publish(Float64(CMD_POS_8))
+ pub_r_gripper.publish(Float64(G_OPEN))
time.sleep(2)
#close gripper
pub_r_arm.publish(JointPosCmd(['r_shoulder_pan_joint','r_shoulder_lift_joint','r_upper_arm_rolr_joint','r_elbow_flex_joint','r_forearm_roll_joint','r_wrist_flex_joint','r_wrist_roll_joint'],[CMD_POS_1,CMD_POS_2+LOWER_Z,CMD_POS_3,CMD_POS_4,CMD_POS_5,CMD_POS_6,CMD_POS_7],[0,0,0,0,0,0,0],0))
- pub_r_gripper.publish(Float64(0.0))
+ pub_r_gripper.publish(Float64(G_CLOSE))
time.sleep(8)
#raise arm
pub_r_arm.publish(JointPosCmd(['r_shoulder_pan_joint','r_shoulder_lift_joint','r_upper_arm_roll_joint','r_elbow_flex_joint','r_forearm_roll_joint','r_wrist_flex_joint','r_wrist_roll_joint'],[CMD_POS_1,CMD_POS_2,CMD_POS_3,CMD_POS_4,CMD_POS_5,CMD_POS_6,CMD_POS_7],[0,0,0,0,0,0,0],0))
- pub_r_gripper.publish(Float64(0.0))
+ pub_r_gripper.publish(Float64(G_CLOSE))
time.sleep(2)
#pan arm
pub_r_arm.publish(JointPosCmd(['r_shoulder_pan_joint','r_shoulder_lift_joint','r_upper_arm_roll_joint','r_elbow_flex_joint','r_forearm_roll_joint','r_wrist_flex_joint','r_wrist_roll_joint'],[CMD_POS_1+PAN_RAD,CMD_POS_2,CMD_POS_3,CMD_POS_4,CMD_POS_5,CMD_POS_6,CMD_POS_7],[0,0,0,0,0,0,0],0))
- pub_r_gripper.publish(Float64(0.0))
+ pub_r_gripper.publish(Float64(G_CLOSE))
time.sleep(2)
#lower arm
pub_r_arm.publish(JointPosCmd(['r_shoulder_pan_joint','r_shoulder_lift_joint','r_upper_arm_roll_joint','r_elbow_flex_joint','r_forearm_roll_joint','r_wrist_flex_joint','r_wrist_roll_joint'],[CMD_POS_1+PAN_RAD,CMD_POS_2+LOWER_Z,CMD_POS_3,CMD_POS_4,CMD_POS_5,CMD_POS_6,CMD_POS_7],[0,0,0,0,0,0,0],0))
- pub_r_gripper.publish(Float64(0.0))
+ pub_r_gripper.publish(Float64(G_CLOSE))
time.sleep(2)
#open gripper
pub_r_arm.publish(JointPosCmd(['r_shoulder_pan_joint','r_shoulder_lift_joint','r_upper_arm_rolr_joint','r_elbow_flex_joint','r_forearm_roll_joint','r_wrist_flex_joint','r_wrist_roll_joint'],[CMD_POS_1+PAN_RAD,CMD_POS_2+LOWER_Z,CMD_POS_3,CMD_POS_4,CMD_POS_5,CMD_POS_6,CMD_POS_7],[0,0,0,0,0,0,0],0))
- pub_r_gripper.publish(Float64(CMD_POS_8))
+ pub_r_gripper.publish(Float64(G_OPEN))
time.sleep(8)
#raise arm
pub_r_arm.publish(JointPosCmd(['r_shoulder_pan_joint','r_shoulder_lift_joint','r_upper_arm_roll_joint','r_elbow_flex_joint','r_forearm_roll_joint','r_wrist_flex_joint','r_wrist_roll_joint'],[CMD_POS_1+PAN_RAD,CMD_POS_2,CMD_POS_3,CMD_POS_4,CMD_POS_5,CMD_POS_6,CMD_POS_7],[0,0,0,0,0,0,0],0))
- pub_r_gripper.publish(Float64(CMD_POS_8))
+ pub_r_gripper.publish(Float64(G_OPEN))
time.sleep(2)
#return arm
pub_r_arm.publish(JointPosCmd(['r_shoulder_pan_joint','r_shoulder_lift_joint','r_upper_arm_roll_joint','r_elbow_flex_joint','r_forearm_roll_joint','r_wrist_flex_joint','r_wrist_roll_joint'],[CMD_POS_1,CMD_POS_2,CMD_POS_3,CMD_POS_4,CMD_POS_5,CMD_POS_6,CMD_POS_7],[0,0,0,0,0,0,0],0))
- pub_r_gripper.publish(Float64(CMD_POS_8))
+ pub_r_gripper.publish(Float64(G_OPEN))
Modified: pkg/trunk/demos/arm_gazebo/l_arm_default_controller.xml
===================================================================
--- pkg/trunk/demos/arm_gazebo/l_arm_default_controller.xml 2009-01-27 23:09:00 UTC (rev 10294)
+++ pkg/trunk/demos/arm_gazebo/l_arm_default_controller.xml 2009-01-27 23:30:15 UTC (rev 10295)
@@ -64,7 +64,7 @@
</controller>
<controller name="l_gripper_controller" topic="l_gripper_controller" type="JointPositionControllerNode">
<joint name="l_gripper_l_finger_joint">
- <pid p="0.5" d="0.000001" i="0.001" iClamp="0.01" />
+ <pid p="10.0" d="0.0" i="0.01" iClamp="0.1" />
</joint>
</controller>
</controllers>
Modified: pkg/trunk/demos/arm_gazebo/r_arm_default_controller.xml
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_arm_default_controller.xml 2009-01-27 23:09:00 UTC (rev 10294)
+++ pkg/trunk/demos/arm_gazebo/r_arm_default_controller.xml 2009-01-27 23:30:15 UTC (rev 10295)
@@ -64,7 +64,7 @@
</controller>
<controller name="r_gripper_controller" topic="r_gripper_controller" type="JointPositionControllerNode">
<joint name="r_gripper_l_finger_joint">
- <pid p="0.5" d="0.000001" i="0.001" iClamp="0.01" />
+ <pid p="10.0" d="0.0" i="0.01" iClamp="0.1" />
</joint>
</controller>
</controllers>
Modified: pkg/trunk/demos/examples_gazebo/table_defs/table_defs.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/table_defs/table_defs.xml 2009-01-27 23:09:00 UTC (rev 10294)
+++ pkg/trunk/demos/examples_gazebo/table_defs/table_defs.xml 2009-01-27 23:30:15 UTC (rev 10295)
@@ -21,10 +21,12 @@
<property name="table_y" value="0.0" />
<property name="table_z" value="0.0" />
- <property name="object_1_height" value="0.12" />
- <property name="object_1_radius" value="0.02" />
+ <property name="object_1_height" value="0.12" />
+ <property name="object_1_radius" value="0.02" />
<property name="object_1_base_height" value="0.02" />
- <property name="object_1_base_width" value="0.15" />
+ <property name="object_1_base_width" value="0.15" />
+ <property name="object_1_mass" value="0.1" />
+ <property name="object_1_mu" value="50.0" />
<property name="M_PI" value="3.1415926535897931" />
@@ -201,11 +203,11 @@
<origin xyz="0 0 ${object_1_base_height}" rpy="0 0 0" />
<joint name="object_1_joint" />
<inertial>
- <mass value="0.01" />
+ <mass value="${object_1_mass/2.0}" />
<com xyz="0 0 ${object_1_height/2}" />
- <inertia ixx="0.001" ixy="0" ixz="0"
- iyy="0.001" iyz="0"
- izz="0.0001" />
+ <inertia ixx="${object_1_mass/20.0}" ixy="0" ixz="0"
+ iyy="${object_1_mass/20.0}" iyz="0"
+ izz="${object_1_mass/200.}" />
</inertial>
<visual>
<origin xyz="0.0 0.0 ${object_1_height/2}" rpy="0 0 0" />
@@ -223,8 +225,9 @@
</geometry>
</collision>
<map name="object_1_collision" flag="gazebo">
- <elem key="mu1" value="5000.0" />
- <elem key="mu2" value="5000.0" />
+ <elem key="selfCollide">true</elem>
+ <elem key="mu1" value="object_1_mu" />
+ <elem key="mu2" value="object_1_mu" />
<elem key="kp" value="100000000.0" />
<elem key="kd" value="1.0" />
</map>
@@ -234,11 +237,11 @@
<origin xyz="${table_x} ${table_y} ${table_z+table_height+0.03}" rpy="0 0 0" />
<joint name="object_1_base_joint" />
<inertial>
- <mass value="0.01" />
+ <mass value="${object_1_mass/2.0}" />
<com xyz="0 0 ${object_1_base_height/2}" />
- <inertia ixx="0.001" ixy="0" ixz="0"
- iyy="0.001" iyz="0"
- izz="0.0001" />
+ <inertia ixx="${object_1_mass/20.0}" ixy="0" ixz="0"
+ iyy="${object_1_mass/20.0}" iyz="0"
+ izz="${object_1_mass/200.}" />
</inertial>
<visual>
<origin xyz="0.0 0.0 ${object_1_base_height/2}" rpy="0 0 0" />
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/empty_10000hz.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/empty_10000hz.world (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/empty_10000hz.world 2009-01-27 23:30:15 UTC (rev 10295)
@@ -0,0 +1,130 @@
+<?xml version="1.0"?>
+
+<gazebo:world
+ 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:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window"
+ xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param"
+ xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
+ xmlns:geo="http://willowgarage.com/xmlschema/#geo"
+ 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:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui"
+ 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" >
+
+ <verbosity>5</verbosity>
+
+ <!-- cfm is 1e-5 for single precision -->
+ <!-- erp is typically .1-.8 -->
+ <!-- here's the global contact cfm/erp -->
+ <physics:ode>
+ <stepTime>0.0001</stepTime>
+ <gravity>0 0 -9.8</gravity>
+ <cfm>0.0000000001</cfm>
+ <erp>0.2</erp>
+ <quickStep>true</quickStep>
+ <quickStepIters>3</quickStepIters>
+ <quickStepW>1.3</quickStepW>
+ </physics:ode>
+
+ <geo:origin>
+ <lat>37.4270909558</lat><lon>-122.077919338</lon>
+ </geo:origin>
+
+ <rendering:gui>
+ <type>fltk</type>
+ <size>1024 800</size>
+ <pos>0 0</pos>
+ <frames>
+ <row height="100%">
+ <camera width="100%">
+ <xyz>0.3 0 3</xyz>
+ <rpy>0 90 90</rpy>
+ </camera>
+ </row>
+ </frames>
+ </rendering:gui>
+
+
+ <rendering:ogre>
+ <ambient>1.0 1.0 1.0 1.0</ambient>
+ <sky>
+ <material>Gazebo/CloudySky</material>
+ </sky>
+ <gazeboPath>media</gazeboPath>
+ <grid>false</grid>
+ <maxUpdateRate>10.</maxUpdateRate>
+ </rendering:ogre>
+
+ <model:physical name="gplane">
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <static>true</static>
+
+ <body:plane name="plane">
+ <geom:plane name="plane">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
+ <normal>0 0 1</normal>
+ <size>51.3 51.3</size>
+ <material>Gazebo/Rocky</material>
+ </geom:plane>
+ </body:plane>
+ </model:physical>
+
+ <model:empty name="factory_model">
+ <controller:factory name="factory_controller">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <interface:factory name="factory_iface"/>
+ </controller:factory>
+ </model:empty>
+
+<!--
+ <model:physical name="walls">
+ <include embedded="false">
+ <xi:include href="tests/willow-walls.model" />
+ </include>
+ </model:physical>
+-->
+<!--
+ <model:physical name="willow_map">
+ <xyz>-25.65 25.65 1.0</xyz>
+ <rpy>180 0 0</rpy>
+ <static>true</static>
+ <body:map name="willow_map_body">
+ <geom:map name="willow_map_geom">
+ <image>willowMap.png</image>
+ <threshold>200</threshold>
+ <granularity>1</granularity>
+ <negative>false</negative>
+ <scale>0.1</scale>
+ <offset>0 0 0</offset>
+ <material>Gazebo/Rocky</material>
+ </geom:map>
+ </body:map>
+ </model:physical>
+-->
+
+
+ <!-- White Directional light -->
+ <!--
+ <model:renderable name="directional_white">
+ <light>
+ <type>directional</type>
+ <direction>0 -0.5 -0.5</direction>
+ <diffuseColor>0.4 0.4 0.4</diffuseColor>
+ <specularColor>0.0 0.0 0.0</specularColor>
+ <attenuation>1 0.0 1.0 0.4</attenuation>
+ </light>
+ </model:renderable>
+ -->
+
+
+</gazebo:world>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml 2009-01-27 23:09:00 UTC (rev 10294)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml 2009-01-27 23:30:15 UTC (rev 10295)
@@ -8,6 +8,9 @@
<property name="gripper_max_angle" value="0.548" />
<property name="gripper_min_angle" value="0.05" />
+ <property name="finger_tip_mu" value="50.0" />
+ <property name="finger_joint_effort_limit" value="10.0" />
+
<macro name="pr2_finger" params="prefix parent">
<!-- Finger proximal digit -->
@@ -15,10 +18,10 @@
<joint name="${prefix}_l_finger_joint" type="revolute">
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
- <calibration reference_position="0" values="1.5 -1.0" />
- <limit effort="10" min="${gripper_min_angle*0}" max="${gripper_max_angle}"
- k_position="1.0" k_velocity="0.1" velocity="0.5"
- safety_length_min="0.05" safety_length_max="0.1" />
+ <calibration reference_position="0" values="1.0 -1.0" />
+ <limit effort="${finger_joint_effort_limit}" min="${gripper_min_angle*0}" max="${gripper_max_angle}"
+ k_position="1.0" k_velocity="1.0" velocity="0.5"
+ safety_length_min="0.0" safety_length_max="0.0" />
<joint_properties damping="0.2" />
</joint>
@@ -79,16 +82,16 @@
<joint name="${prefix}_r_finger_joint" type="revolute">
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
- <calibration reference_position="0" values="1.5 -1.0" />
- <limit effort="10" min="${-gripper_max_angle}" max="${-gripper_min_angle*0}"
- k_position="0" k_velocity="0" velocity="0.5"
+ <calibration reference_position="0" values="1.0 -1.0" />
+ <limit effort="${finger_joint_effort_limit}" min="${-gripper_max_angle}" max="${-gripper_min_angle*0}"
+ k_position="0" k_velocity="0" velocity="0.0"
safety_length_min="0.0" safety_length_max="0.0" />
<joint_properties damping="0.2" />
<mimic name="${prefix}_l_finger_joint" mult="${-1}" offset="0"/>
<map name="${prefix}_r_finger_mimic" flag="gazebo">
- <elem key="mimicKp">1.0</elem>
- <elem key="mimicKd">1.0</elem>
- <elem key="mimicFMax">50.0</elem> <!-- ode bug: this force is set for _l_finger_joint -->
+ <elem key="mimicKp" value="20.0" />
+ <elem key="mimicKd" value="10.0" />
+ <elem key="mimicFMax" value="10.0" />
</map>
</joint>
@@ -149,16 +152,16 @@
<joint name="${prefix}_l_finger_tip_joint" type="revolute">
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
- <calibration reference_position="0" values="1.5 -1.0" />
- <limit effort="10" min="${-gripper_max_angle}" max="${-gripper_min_angle*0}"
- k_position="0" k_velocity="0" velocity="0.5"
+ <calibration reference_position="0" values="1.0 -1.0" />
+ <limit effort="${finger_joint_effort_limit}" min="${-gripper_max_angle}" max="${-gripper_min_angle*0}"
+ k_position="0" k_velocity="0" velocity="0.0"
safety_length_min="0.0" safety_length_max="0.0" />
<joint_properties damping="0.002" />
<mimic name="${prefix}_l_finger_joint" mult="-1" offset="0"/>
<map name="${prefix}_l_finger_tip_mimic" flag="gazebo">
- <elem key="mimicKp">2.0</elem>
- <elem key="mimicKd">1.0</elem>
- <elem key="mimicFMax">10.0</elem>
+ <elem key="mimicKp" value="20.0" />
+ <elem key="mimicKd" value="10.0" />
+ <elem key="mimicFMax" value="10.0" />
</map>
</joint>
@@ -209,8 +212,8 @@
</map>
<map name="${prefix}_l_finger_tip_gravity" flag="gazebo">
<elem key="turnGravityOff">true</elem>
- <elem key="mu1" value="50.0" />
- <elem key="mu2" value="50.0" />
+ <elem key="mu1" value="${finger_tip_mu}" />
+ <elem key="mu2" value="${finger_tip_mu}" />
<elem key="kp" value="100000000.0" />
<elem key="kd" value="1.0" />
</map>
@@ -221,15 +224,15 @@
<joint name="${prefix}_r_finger_tip_joint" type="revolute">
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
- <calibration reference_position="0" values="1.5 -1.0" />
- <limit effort="10" min="${gripper_min_angle*0}" max="${gripper_max_angle}"
- k_position="0" k_velocity="0" velocity="0.5"
+ <calibration reference_position="0" values="1.0 -1.0" />
+ <limit effort="${finger_joint_effort_limit}" min="${gripper_min_angle*0}" max="${gripper_max_angle}"
+ k_position="0" k_velocity="0" velocity="0.0"
safety_length_min="0.0" safety_length_max="0.0" />
<joint_properties damping="0.002" />
<mimic name="${prefix}_l_finger_joint" mult="1" offset="0"/>
<map name="${prefix}_r_finger_tip_mimic" flag="gazebo">
- <elem key="mimicKp" value="2.0" />
- <elem key="mimicKd" value="1.0" />
+ <elem key="mimicKp" value="20.0" />
+ <elem key="mimicKd" value="10.0" />
<elem key="mimicFMax" value="10.0" />
</map>
</joint>
@@ -281,8 +284,8 @@
</map>
<map name="${prefix}_r_finger_tip_gravity" flag="gazebo">
<elem key="turnGravityOff">true</elem>
- <elem key="mu1" value="50.0" />
- <elem key="mu2" value="50.0" />
+ <elem key="mu1" value="${finger_tip_mu}" />
+ <elem key="mu2" value="${finger_tip_mu}" />
<elem key="kp" value="100000000.0" />
<elem key="kd" value="1.0" />
</map>
@@ -299,6 +302,14 @@
<frameName>map</frameName>
<interface:position name="p3d_${prefix}_l_finger_position_iface" />
</controller:ros_p3d>
+ <controller:ros_f3d name="f3d_${prefix}_l_finger_controller" plugin="libros_f3d.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>${prefix}_l_finger_link</bodyName>
+ <topicName>${prefix}_l_finger_force_ground_truth</topicName>
+ <frameName>${prefix}_l_finger_link</frameName>
+ <interface:position name="f3d_${prefix}_l_finger_force_iface" />
+ </controller:ros_f3d>
</verbatim>
</map>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
