Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [858] pkg/trunk
|
From: <hsu...@us...> - 2008-06-19 21:42:58
|
Revision: 858
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=858&view=rev
Author: hsujohnhsu
Date: 2008-06-19 14:42:52 -0700 (Thu, 19 Jun 2008)
Log Message:
-----------
added some components for torque control.
partial updates to libpr2HW. compiled but not used yet.
add robot_force.world for torque_control.
Modified Paths:
--------------
pkg/trunk/demos/2dnav-gazebo/world/pr2.model
pkg/trunk/demos/2dnav-gazebo/world/robot.world
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
pkg/trunk/simulators/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh
pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc
pkg/trunk/simulators/rosgazebo/rosgazebo.cc
pkg/trunk/simulators/rosgazebo/setup.bash
pkg/trunk/simulators/rosgazebo/setup.tcsh
Added Paths:
-----------
pkg/trunk/demos/2dnav-gazebo/world/robot_force.world
Modified: pkg/trunk/demos/2dnav-gazebo/world/pr2.model
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/world/pr2.model 2008-06-19 17:19:53 UTC (rev 857)
+++ pkg/trunk/demos/2dnav-gazebo/world/pr2.model 2008-06-19 21:42:52 UTC (rev 858)
@@ -1203,7 +1203,7 @@
<body2>front_left_wheel_l</body2>
<anchor>front_left_wheel_l</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1212,7 +1212,7 @@
<body2>front_left_wheel_r</body2>
<anchor>front_left_wheel_r</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1221,7 +1221,7 @@
<body2>front_right_wheel_l</body2>
<anchor>front_right_wheel_l</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1230,7 +1230,7 @@
<body2>front_right_wheel_r</body2>
<anchor>front_right_wheel_r</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1239,7 +1239,7 @@
<body2>rear_left_wheel_l</body2>
<anchor>rear_left_wheel_l</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1248,7 +1248,7 @@
<body2>rear_left_wheel_r</body2>
<anchor>rear_left_wheel_r</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1257,7 +1257,7 @@
<body2>rear_right_wheel_l</body2>
<anchor>rear_right_wheel_l</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1266,7 +1266,7 @@
<body2>rear_right_wheel_r</body2>
<anchor>rear_right_wheel_r</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1278,7 +1278,7 @@
<body2>base_body</body2>
<anchor>front_left_caster_body</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1287,7 +1287,7 @@
<body2>base_body</body2>
<anchor>front_right_caster_body</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1296,7 +1296,7 @@
<body2>base_body</body2>
<anchor>rear_left_caster_body</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1305,7 +1305,7 @@
<body2>base_body</body2>
<anchor>rear_right_caster_body</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
Modified: pkg/trunk/demos/2dnav-gazebo/world/robot.world
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/world/robot.world 2008-06-19 17:19:53 UTC (rev 857)
+++ pkg/trunk/demos/2dnav-gazebo/world/robot.world 2008-06-19 21:42:52 UTC (rev 858)
@@ -360,44 +360,51 @@
<joint name="neck_yaw_joint">
<saturationTorque>100</saturationTorque>
<pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="head_tilt_joint">
<saturationTorque>10</saturationTorque>
<pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="hokuyo_pitch_joint">
<saturationTorque>1000</saturationTorque>
<pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="head_cam_left_yaw_joint">
<saturationTorque>100</saturationTorque>
<pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="head_cam_left_pitch_joint">
<saturationTorque>100</saturationTorque>
<pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="head_cam_right_yaw_joint">
<saturationTorque>100</saturationTorque>
<pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="head_cam_right_pitch_joint">
<saturationTorque>100</saturationTorque>
<pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<interface:pr2array name="pr2_head_iface"/>
</controller:pr2_actarray>
@@ -411,72 +418,84 @@
<pGain>2</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="front_left_wheel_l_drive">
<saturationTorque>5</saturationTorque>
<pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="front_left_wheel_r_drive">
<saturationTorque>5</saturationTorque>
<pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="front_right_caster_steer">
<saturationTorque>1000</saturationTorque>
<pGain>2</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="front_right_wheel_l_drive">
<saturationTorque>5</saturationTorque>
<pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="front_right_wheel_r_drive">
<saturationTorque>5</saturationTorque>
<pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="rear_left_caster_steer">
<saturationTorque>1000</saturationTorque>
<pGain>2</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="rear_left_wheel_l_drive">
<saturationTorque>5</saturationTorque>
<pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="rear_left_wheel_r_drive">
<saturationTorque>5</saturationTorque>
<pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="rear_right_caster_steer">
<saturationTorque>1000</saturationTorque>
<pGain>2</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="rear_right_wheel_l_drive">
<saturationTorque>5</saturationTorque>
<pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="rear_right_wheel_r_drive">
<saturationTorque>5</saturationTorque>
<pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<!-- ========================================= -->
<joint name="torso_spine_slider">
@@ -484,6 +503,7 @@
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<!-- ========================================= -->
<joint name="shoulder_pan_left">
@@ -491,42 +511,49 @@
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="upperarm_pitch_joint_left">
<saturationTorque>1000</saturationTorque>
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="upperarm_roll_joint_left">
<saturationTorque>1000</saturationTorque>
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="elbow_pitch_joint_left">
<saturationTorque>100</saturationTorque>
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="forearm_roll_joint_left">
<saturationTorque>100</saturationTorque>
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="wrist_pitch_joint_left">
<saturationTorque>100</saturationTorque>
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="palm_roll_joint_left">
<saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
- <pGain>1</pGain>
+ <iClamp>0</iClamp>
</joint>
<!-- ========================================= -->
<joint name="shoulder_pan_right">
@@ -534,42 +561,49 @@
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="upperarm_pitch_joint_right">
<saturationTorque>1000</saturationTorque>
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="upperarm_roll_joint_right">
<saturationTorque>1000</saturationTorque>
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="elbow_pitch_joint_right">
<saturationTorque>1000</saturationTorque>
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="forearm_roll_joint_right">
<saturationTorque>100</saturationTorque>
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="wrist_pitch_joint_right">
<saturationTorque>100</saturationTorque>
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="palm_roll_joint_right">
<saturationTorque>100</saturationTorque>
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<!-- ========================================= -->
<interface:pr2array name="pr2_iface"/>
Added: pkg/trunk/demos/2dnav-gazebo/world/robot_force.world
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/world/robot_force.world (rev 0)
+++ pkg/trunk/demos/2dnav-gazebo/world/robot_force.world 2008-06-19 21:42:52 UTC (rev 858)
@@ -0,0 +1,668 @@
+<?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 -->
+ <physics:ode>
+ <stepTime>0.01</stepTime>
+ <gravity>0 0 -9.8</gravity>
+ <cfm>0.001</cfm>
+ <erp>0.8</erp>
+ </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>
+ </rendering:gui>
+
+
+ <rendering:ogre>
+ <ambient>1.0 1.0 1.0 1.0</ambient>
+ <sky>
+ <material>Gazebo/CloudySky</material>
+ </sky>
+ <gazeboPath>media</gazeboPath>
+ <grid>off</grid>
+ <desiredFPS>10</desiredFPS>
+ </rendering:ogre>
+
+
+<!--
+ <model:physical name="gbuilding">
+ <body:heightmap name="building">
+ <geom:heightmap name="building">
+ <image>map.png</image>
+ <worldTexture>map2.jpg</worldTexture>
+ <size>2 2 0.5</size>
+ </geom:heightmap>
+ </body:heightmap>
+ </model:physical>
+-->
+ <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">
+ <normal>0 0 1</normal>
+ <size>51.3 51.3</size>
+ <!-- map3.png -->
+ <material>PR2/floor_texture</material>
+ </geom:plane>
+ </body:plane>
+ </model:physical>
+<!--
+-->
+
+
+
+ <!-- The camera -->
+ <model:physical name="cam1_model">
+ <xyz> 3.0 0.0 1.0</xyz>
+ <rpy> 0.0 0.0 180.0</rpy>
+ <static>true</static>
+
+ <body:empty name="cam1_body">
+ <sensor:camera name="cam1_sensor">
+ <imageSize>1024 800</imageSize>
+ <hfov>90</hfov>
+ <nearClip>0.1</nearClip>
+ <farClip>20</farClip>
+ <saveFrames>false</saveFrames>
+ <saveFramePath>frames</saveFramePath>
+ <controller:generic_camera name="cam1_controller">
+ <updateRate>10.0</updateRate>
+ <interface:camera name="cam1_iface_0" />
+ </controller:generic_camera>
+ </sensor:camera>
+ </body:empty>
+ </model:physical>
+
+
+ <!-- test laser model -->
+ <!--
+ <model:physical name="test_laser_model">
+ <xyz> -2.0 0.0 0.15</xyz>
+ <rpy> 0.0 0.0 90.0</rpy>
+ <static>true</static>
+ <body:box name="test_laser_body">
+ <xyz>0.0 0.0 0.0</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+
+ <geom:box name="test_laser_box">
+ <xyz>0.0 0.0 -0.05</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.05 0.15 0.02</size>
+ <mass>0.0</mass>
+ <visual>
+ <scale>0.05 0.15 0.02</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Blue</material>
+ </visual>
+ </geom:box>
+
+ <sensor:ray name="laser_1">
+ <rayCount>100</rayCount>
+ <rangeCount>100</rangeCount>
+ <laserCount>1</laserCount>
+ <origin>0.0 0.0 0.10</origin>
+
+ <minAngle>-135</minAngle>
+ <maxAngle>135</maxAngle>
+
+ <minRange>0.036</minRange>
+ <maxRange>10.0</maxRange>
+
+ <updateRate>10.0</updateRate>
+
+ <controller:sicklms200_laser name="laser_controller_1">
+ <interface:laser name="laser_iface_1"/>
+ <updateRate>10</updateRate>
+ </controller:sicklms200_laser>
+ </sensor:ray>
+
+ </body:box>
+ </model:physical>
+ -->
+
+ <!-- The "desk"-->
+ <model:physical name="desk1_model">
+ <xyz> 2.28 -0.21 -0.10</xyz>
+ <rpy> 0.0 0.0 0.00</rpy>
+ <body:box name="desk1_legs_body">
+ <geom:box name="desk1_legs_geom">
+ <xyz> 0.0 0.0 0.50</xyz>
+ <mesh>default</mesh>
+ <size>0.5 1.0 0.75</size>
+ <mass> 10.0</mass>
+ <cfm>0.000001</cfm>
+ <erp>0.8</erp>
+ <visual>
+ <size> 0.5 1.0 0.75</size>
+ <material>Gazebo/Rocky</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ <geom:box name="desk1_top_geom">
+ <xyz> 0.0 0.0 0.90</xyz>
+ <mesh>default</mesh>
+ <size>0.75 1.5 0.05</size>
+ <mass> 10.0</mass>
+ <cfm>0.000001</cfm>
+ <erp>0.8</erp>
+ <visual>
+ <size> 0.75 1.5 0.05</size>
+ <material>Gazebo/Rocky</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <!-- The second "desk"-->
+ <model:physical name="desk2_model">
+ <xyz> 2.25 -3.0 -0.10</xyz>
+ <rpy> 0.0 0.0 0.00</rpy>
+ <body:box name="desk2_legs_body">
+ <geom:box name="desk2_legs_geom">
+ <xyz> 0.0 0.0 0.50</xyz>
+ <mesh>default</mesh>
+ <size>0.5 1.0 0.75</size>
+ <mass> 10.0</mass>
+ <cfm>0.000001</cfm>
+ <erp>0.8</erp>
+ <visual>
+ <size> 0.5 1.0 0.75</size>
+ <material>Gazebo/Rocky</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ <geom:box name="desk2_top_geom">
+ <xyz> 0.0 0.0 0.90</xyz>
+ <mesh>default</mesh>
+ <size>0.75 1.5 0.05</size>
+ <mass> 10.0</mass>
+ <cfm>0.001</cfm>
+ <erp>0.8</erp>
+ <visual>
+ <size> 0.75 1.5 0.05</size>
+ <material>Gazebo/Rocky</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <!-- The small cylinder "cup" -->
+ <model:physical name="cylinder1_model">
+ <xyz> 2.5 0.0 0.9</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <cfm>0.001</cfm>
+ <erp>0.5</erp>
+ <body:cylinder name="cylinder1_body">
+ <geom:cylinder name="cylinder1_geom">
+ <mesh>default</mesh>
+ <size>0.025 0.075</size>
+ <mass> 0.05</mass>
+ <visual>
+ <size> 0.05 0.05 0.075</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_cylinder</mesh>
+ </visual>
+ </geom:cylinder>
+ <geom:box name="cylinder1_base_geom">
+ <mesh>default</mesh>
+ <xyz>0.0 0.0 -0.033</xyz>
+ <size>0.05 0.05 0.01</size>
+ <mass> 0.01</mass>
+ <visual>
+ <size> 0.05 0.05 0.01</size>
+ <material>Gazebo/Fish</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:cylinder>
+ </model:physical>
+
+
+ <!-- The small ball -->
+ <model:physical name="sphere1_model">
+ <xyz> 2.5 -2.8 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <body:sphere name="sphere1_body">
+ <geom:sphere name="sphere1_geom">
+ <mesh>default</mesh>
+ <size> 0.15</size>
+ <mass> 1.0</mass>
+ <cfm>0.000001</cfm>
+ <erp>0.8</erp>
+ <visual>
+ <size> 0.3 0.3 0.3</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_sphere</mesh>
+ </visual>
+ </geom:sphere>
+ </body:sphere>
+ </model:physical>
+
+ <!-- The large ball map3.png -->
+ <model:physical name="sphere2_model">
+ <xyz> 5.85 4.35 1.55</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <body:sphere name="sphere2_body">
+ <geom:sphere name="sphere2_geom">
+ <mesh>default</mesh>
+ <size> 1.0</size>
+ <mass> 1.0</mass>
+ <cfm>0.000001</cfm>
+ <erp>0.8</erp>
+ <visual>
+ <size> 2.0 2.0 2.0</size>
+ <material>Gazebo/Rocky</material>
+ <mesh>unit_sphere</mesh>
+ </visual>
+ </geom:sphere>
+ </body:sphere>
+ </model:physical>
+
+ <!-- The wall in front map3.png -->
+ <model:physical name="wall_2_model">
+ <xyz> 11.6 -1.55 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="wall_2_body">
+ <geom:box name="wall_2_geom">
+ <mesh>default</mesh>
+ <size>2.1 32.8 2.0</size>
+ <visual>
+ <size>2.1 32.8 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <!-- The wall behind -->
+ <model:physical name="wall_1_model">
+ <xyz> -11.3 -1.45 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="wall_1_body">
+ <geom:box name="wall_1_geom">
+ <mesh>default</mesh>
+ <size>0.4 24.0 2.0</size>
+ <visual>
+ <size>0.4 24.0 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <!-- The wall 3 -->
+ <model:physical name="wall_3_model">
+ <xyz> 6.7 8.05 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="wall_3_body">
+ <geom:box name="wall_3_geom">
+ <mesh>default</mesh>
+ <size>7.5 1.2 2.0</size>
+ <visual>
+ <size>7.5 1.2 2.0</size>
+ <material>Gazebo/Chrome</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+
+
+ <model:physical name="robot_model1">
+ <xyz>0.0 0.0 0.262</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+
+ <!-- base, torso and arms -->
+ <include embedded="true">
+ <xi:include href="pr2.model" />
+ </include>
+
+ <controller:pr2_actarray name="pr2_head_controller" plugin="libPr2_Actarray.so">
+ <updateRate>100.0</updateRate>
+ <joint name="neck_yaw_joint">
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="head_tilt_joint">
+ <saturationTorque>10</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="hokuyo_pitch_joint">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="head_cam_left_yaw_joint">
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="head_cam_left_pitch_joint">
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="head_cam_right_yaw_joint">
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="head_cam_right_pitch_joint">
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <interface:pr2array name="pr2_head_iface"/>
+ </controller:pr2_actarray>
+
+ <!-- use actarray for the rest of the model -->
+
+ <controller:pr2_actarray name="pr2_controller" plugin="libPr2_Actarray.so">
+ <updateRate>100.0</updateRate>
+ <joint name="front_left_caster_steer">
+ <saturationTorque>3</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>.5</iGain>
+ <iClamp>3</iClamp>
+ </joint>
+ <joint name="front_left_wheel_l_drive">
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
+ </joint>
+ <joint name="front_left_wheel_r_drive">
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
+ </joint>
+ <joint name="front_right_caster_steer">
+ <saturationTorque>3</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>.5</iGain>
+ <iClamp>3</iClamp>
+ </joint>
+ <joint name="front_right_wheel_l_drive">
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="front_right_wheel_r_drive">
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
+ </joint>
+ <joint name="rear_left_caster_steer">
+ <saturationTorque>3</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>.1</iGain>
+ <iClamp>1</iClamp>
+ </joint>
+ <joint name="rear_left_wheel_l_drive">
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
+ </joint>
+ <joint name="rear_left_wheel_r_drive">
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
+ </joint>
+ <joint name="rear_right_caster_steer">
+ <saturationTorque>3</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>.1</iGain>
+ <iClamp>1</iClamp>
+ </joint>
+ <joint name="rear_right_wheel_l_drive">
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
+ </joint>
+ <joint name="rear_right_wheel_r_drive">
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
+ </joint>
+ <!-- ========================================= -->
+ <joint name="torso_spine_slider">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <!-- ========================================= -->
+ <joint name="shoulder_pan_left">
+ <saturationTorque>50000</saturationTorque>
+ <pGain>20000</pGain>
+ <dGain>0</dGain>
+ <iGain>300</iGain>
+ <iClamp>1000</iClamp>
+ </joint>
+ <joint name="upperarm_pitch_joint_left">
+ <saturationTorque>20000</saturationTorque>
+ <pGain>20000</pGain>
+ <dGain>0</dGain>
+ <iGain>100</iGain>
+ <iClamp>1000</iClamp>
+ </joint>
+ <joint name="upperarm_roll_joint_left">
+ <saturationTorque>10000</saturationTorque>
+ <pGain>500</pGain>
+ <dGain>0</dGain>
+ <iGain>10</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="elbow_pitch_joint_left">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>500</pGain>
+ <dGain>0</dGain>
+ <iGain>10</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="forearm_roll_joint_left">
+ <saturationTorque>100</saturationTorque>
+ <pGain>100</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="wrist_pitch_joint_left">
+ <saturationTorque>100</saturationTorque>
+ <pGain>100</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="palm_roll_joint_left">
+ <saturationTorque>100</saturationTorque>
+ <pGain>100</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <!-- ========================================= -->
+ <joint name="shoulder_pan_right">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>5000</pGain>
+ <dGain>0</dGain>
+ <iGain>500</iGain>
+ <iClamp>1000</iClamp>
+ </joint>
+ <joint name="upperarm_pitch_joint_right">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>3000</pGain>
+ <dGain>0</dGain>
+ <iGain>300</iGain>
+ <iClamp>1000</iClamp>
+ </joint>
+ <joint name="upperarm_roll_joint_right">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>500</pGain>
+ <dGain>0</dGain>
+ <iGain>10</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="elbow_pitch_joint_right">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>500</pGain>
+ <dGain>0</dGain>
+ <iGain>10</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="forearm_roll_joint_right">
+ <saturationTorque>100</saturationTorque>
+ <pGain>100</pGain>
+ <dGain>0</dGain>
+ <iGain>10</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="wrist_pitch_joint_right">
+ <saturationTorque>100</saturationTorque>
+ <pGain>100</pGain>
+ <dGain>0</dGain>
+ <iGain>10</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="palm_roll_joint_right">
+ <saturationTorque>100</saturationTorque>
+ <pGain>100</pGain>
+ <dGain>0</dGain>
+ <iGain>10</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <!-- ========================================= -->
+ <interface:pr2array name="pr2_iface"/>
+ <!-- ========================================= -->
+ </controller:pr2_actarray>
+
+ <controller:pr2_gripper name="pr2_gripper_left_controller" plugin="libPr2_Gripper.so">
+ <updateRate>100.0</updateRate>
+ <leftJoint>finger_2_slider_left</leftJoint>
+ <rightJoint>finger_1_slider_left</rightJoint>
+ <gripperForce>100.0</gripperForce>
+ <pGain>1.0</pGain>
+ <dGain>0.0</dGain>
+ <iGain>0.0</iGain>
+ <interface:pr2gripper name="pr2_gripper_left_iface"/>
+ </controller:pr2_gripper>
+
+ <controller:pr2_gripper name="pr2_gripper_right_controller" plugin="libPr2_Gripper.so">
+ <updateRate>100.0</updateRate>
+ <leftJoint>finger_2_slider_right</leftJoint>
+ <rightJoint>finger_1_slider_right</rightJoint>
+ <gripperForce>100.0</gripperForce>
+ <pGain>1.0</pGain>
+ <dGain>0.0</dGain>
+ <iGain>0.0</iGain>
+ <interface:pr2gripper name="pr2_gripper_right_iface"/>
+ </controller:pr2_gripper>
+
+ <controller:P3D name="p3d_right_wrist_controller" plugin="libP3D.so">
+ <updateRate>100.0</updateRate>
+ <bodyName>palm_right</bodyName>
+ <interface:position name="p3d_right_wrist_position"/>
+ </controller:P3D>
+
+ <controller:P3D name="p3d_left_wrist_controller" plugin="libP3D.so">
+ <updateRate>100.0</updateRate>
+ <bodyName>palm_left</bodyName>
+ <interface:position name="p3d_left_wrist_position"/>
+ </controller:P3D>
+
+ <controller:P3D name="p3d_base_controller" plugin="libP3D.so">
+ <updateRate>100.0</updateRate>
+ <bodyName>base_body</bodyName>
+ <interface:position name="p3d_base_position"/>
+ </controller:P3D>
+
+ </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/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-19 17:19:53 UTC (rev 857)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-19 21:42:52 UTC (rev 858)
@@ -820,9 +820,23 @@
{
case PR2_RIGHT_ARM:
armControlMode[0] = mode;
+ SetJointControlMode(ARM_L_PAN , PD_CONTROL);
+ SetJointControlMode(ARM_L_SHOULDER_PITCH , PD_CONTROL);
+ SetJointControlMode(ARM_L_SHOULDER_ROLL , PD_CONTROL);
+ SetJointControlMode(ARM_L_ELBOW_PITCH , PD_CONTROL);
+ SetJointControlMode(ARM_L_ELBOW_ROLL , PD_CONTROL);
+ SetJointControlMode(ARM_L_WRIST_PITCH , PD_CONTROL);
+ SetJointControlMode(ARM_L_WRIST_ROLL , PD_CONTROL);
break;
case PR2_LEFT_ARM:
armControlMode[1] = mode;
+ SetJointControlMode(ARM_R_PAN , PD_CONTROL);
+ SetJointControlMode(ARM_R_SHOULDER_PITCH , PD_CONTROL);
+ SetJointControlMode(ARM_R_SHOULDER_ROLL , PD_CONTROL);
+ SetJointControlMode(ARM_R_ELBOW_PITCH , PD_CONTROL);
+ SetJointControlMode(ARM_R_ELBOW_ROLL , PD_CONTROL);
+ SetJointControlMode(ARM_R_WRIST_PITCH , PD_CONTROL);
+ SetJointControlMode(ARM_R_WRIST_ROLL , PD_CONTROL);
break;
default:
break;
@@ -1403,8 +1417,8 @@
GetJointServoActual((PR2_JOINT_ID)(CASTER_FL_STEER+3*ii),¤tAngle[ii],¤tRateAct[ii]);
currentError[ii] = fabs(shortest_angular_distance(currentCmd[ii] , currentAngle[ii]));
//std::cout << " ii " << ii << " e " << currentError[ii] << std::endl;
- fprintf(stdout," ii %d e %.3f \n", ii , currentError[ii]);
errorTotal = errorTotal + currentError[ii];
+ fprintf(stdout," ii %d e %.3f - %.3f \n", ii , currentError[ii],errorTotal);
}
fprintf(stdout," ----------------------------------\n");
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h 2008-06-19 17:19:53 UTC (rev 857)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h 2008-06-19 21:42:52 UTC (rev 858)
@@ -49,7 +49,7 @@
{
private: int numJoints;
// we'll declare a pid controller for each hinger/slider/... joint
- private: std::vector<Pid*> pids;
+ private: std::vector<Pid> pids;
/*! \fn
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-06-19 17:19:53 UTC (rev 857)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-06-19 21:42:52 UTC (rev 858)
@@ -72,7 +72,8 @@
PR2_ERROR_CODE PR2HW::AddJoint(PR2_JOINT_ID id)
{
-
+ Pid tmpPid;
+ this->pids.push_back(tmpPid);
return PR2_ALL_OK;
}
Modified: pkg/trunk/simulators/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh
===================================================================
--- pkg/trunk/simulators/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh 2008-06-19 17:19:53 UTC (rev 857)
+++ pkg/trunk/simulators/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh 2008-06-19 21:42:52 UTC (rev 858)
@@ -99,10 +99,13 @@
// number of joints in this array
private: int num_joints;
+ // save last time for dt calc for pid's
+ private: double lastTime;
+
// using the actuator class
// this is where the PR2 API links to the Simulator
// same set of calls used for the hardware
- private: PR2::PR2HW actuatorAPI;
+ private: PR2::PR2HW actuatorAPI; //*actuatorAPI[GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
};
Modified: pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc
===================================================================
--- pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc 2008-06-19 17:19:53 UTC (rev 857)
+++ pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc 2008-06-19 21:42:52 UTC (rev 858)
@@ -150,24 +150,31 @@
}
this->myIface->data->actuators[i].saturationTorque = jNode->GetDouble("saturationTorque",0.0,1);
- this->myIface->data->actuators[i].pGain = jNode->GetDouble("pGain",0.0,1);
- this->myIface->data->actuators[i].iGain = jNode->GetDouble("iGain",0.0,1);
- this->myIface->data->actuators[i].dGain = jNode->GetDouble("dGain",0.0,1);
+ this->myIface->data->actuators[i].pGain = jNode->GetDouble("pGain" ,0.0,1);
+ this->myIface->data->actuators[i].iGain = jNode->GetDouble("iGain" ,0.0,1);
+ this->myIface->data->actuators[i].dGain = jNode->GetDouble("dGain" ,0.0,1);
+ this->myIface->data->actuators[i].iClamp = jNode->GetDouble("iClamp",0.0,1);
//this->myIface->data->actuators[i].cmdPosition = 0.0;
//this->myIface->data->actuators[i].cmdSpeed = 0.0;
+ // TODO: link actuatorAPI object to iface
// add a actuator API
if (name == "front_left_caster_steer")
+ {
this->actuatorAPI.AddJoint(PR2::CASTER_FL_STEER);
+ }
if (name == "front_left_wheel_l_drive")
+ {
this->actuatorAPI.AddJoint(PR2::CASTER_FL_DRIVE_L);
+ }
// init a new pid for this joint
this->pids[i] = new Pid();
// get time
this->myIface->data->actuators[i].timestamp = Simulator::Instance()->GetSimTime();
- // set default control mode to PD
+ // set default control mode to:
+ //this->myIface->data->actuators[i].controlMode = PR2::TORQUE_CONTROL;
this->myIface->data->actuators[i].controlMode = PR2::PD_CONTROL;
jNode = jNode->GetNext("joint");
@@ -190,14 +197,15 @@
this->pids[count]->InitPid( this->myIface->data->actuators[count].pGain,
this->myIface->data->actuators[count].iGain,
this->myIface->data->actuators[count].dGain,
- 1.0,
- -1.0
+ this->myIface->data->actuators[count].iClamp,
+ -this->myIface->data->actuators[count].iClamp
);
this->pids[count]->SetCurrentCmd(0);
// as a first hack, initialize to zero velocity and saturation torque
this->joints[count]->SetParam( dParamVel , this->pids[count]->GetCurrentCmd());
this->joints[count]->SetParam( dParamFMax, this->myIface->data->actuators[count].saturationTorque );
}
+ lastTime = Simulator::Instance()->GetSimTime();
}
////////////////////////////////////////////////////////////////////////////////
@@ -268,9 +276,16 @@
{
case PR2::TORQUE_CONTROL :
// No fancy controller, just pass the commanded torque/force in (we are not modeling the motors for now)
- sjoint->SetSliderForce(this->myIface->data->actuators[count].cmdEffectorForce);
+ positionError = cmdPosition - sjoint->GetPosition();
+ speedError = cmdSpeed - sjoint->GetPositionRate();
+ //std::cout << "slider e:" << speedError << " + " << positionError << std::endl;
+ currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
+ currentCmd = (currentCmd > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : currentCmd;
+ currentCmd = (currentCmd < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : currentCmd;
+ sjoint->SetSliderForce(currentCmd);
+ //sjoint->SetSliderForce(this->myIface->data->actuators[count].cmdEffectorForce);
break;
- case PR2::PD_CONTROL :
+ case PR2::PD_CONTROL : // velocity control
//if (cmdPosition > sjoint->GetHighStop())
// cmdPosition = sjoint->GetHighStop();
//else if (cmdPosition < sjoint->GetLowStop())
@@ -279,7 +294,7 @@
positionError = cmdPosition - sjoint->GetPosition();
speedError = cmdSpeed - sjoint->GetPositionRate();
//std::cout << "slider e:" << speedError << " + " << positionError << std::endl;
- currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime);
+ currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
sjoint->SetParam( dParamVel , currentCmd );
sjoint->SetParam( dParamFMax, this->myIface->data->actuators[count].saturationTorque );
@@ -306,7 +321,14 @@
{
case PR2::TORQUE_CONTROL:
// No fancy controller, just pass the commanded torque/force in (we are not modeling the motors for now)
- hjoint->SetTorque(this->myIface->data->actuators[count].cmdEffectorForce);
+ positionError = ModNPi2Pi(cmdPosition - hjoint->GetAngle());
+ speedError = cmdSpeed - hjoint->GetAngleRate();
+ currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
+ std::cout << "hinge err:" << positionError << " cmd: " << currentCmd << std::endl;
+ // limit torque
+ currentCmd = (currentCmd > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : currentCmd;
+ currentCmd = (currentCmd < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : currentCmd;
+ hjoint->SetTorque(currentCmd);
break;
case PR2::PD_CONTROL:
//if (cmdPosition > hjoint->GetHighStop())
@@ -317,7 +339,7 @@
positionError = ModNPi2Pi(cmdPosition - hjoint->GetAngle());
speedError = cmdSpeed - hjoint->GetAngleRate();
//std::cout << "hinge e:" << speedError << " + " << positionError << std::endl;
- currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime);
+ currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
hjoint->SetParam( dParamVel, currentCmd );
hjoint->SetParam( dParamFMax,this->myIface->data->actuators[count].saturationTorque );
@@ -342,6 +364,7 @@
this->myIface->data->new_cmd = 0;
this->myIface->Unlock();
+ this->lastTime = currentTime;
}
////////////////////////////////////////////////////////////////////////////////
Modified: pkg/trunk/simulators/rosgazebo/rosgazebo.cc
===================================================================
--- pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-19 17:19:53 UTC (rev 857)
+++ pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-19 21:42:52 UTC (rev 858)
@@ -223,12 +223,11 @@
// when running with the 2dnav stack, we need to refrain from moving when steering angles are off.
// when operating with the keyboard, we need instantaneous setting of both velocity and angular velocity.
- // std::cout << "received cmd: vx " << this->velMsg.vx << " vw " << this->velMsg.vw
- // << " vxsmoo " << this->vxSmooth << " vxsmoo " << this->vwSmooth
- // << " | steer erros: " << this->myPR2->BaseSteeringAngleError() << " - " << M_PI/100.0
- // << std::endl;
+ std::cout << "received cmd: vx " << this->velMsg.vx << " vw " << this->velMsg.vw
+ << " vxsmoo " << this->vxSmooth << " vxsmoo " << this->vwSmooth
+ << " | steer erros: " << this->myPR2->BaseSteeringAngleError() << " - " << M_PI/100.0
+ << std::endl;
-
// 2dnav: if steering angle is wrong, don't move or move slowly
if (this->myPR2->BaseSteeringAngleError() > M_PI/100.0)
{
@@ -266,7 +265,12 @@
this->myPR2->InitializeRobot();
// Set control mode for the base
this->myPR2->SetBaseControlMode(PR2::PR2_CARTESIAN_CONTROL);
+ // this->myPR2->SetJointControlMode(PR2::CASTER_FL_STEER, PR2::TORQUE_CONTROL);
+ // this->myPR2->SetJointControlMode(PR2::CASTER_FR_STEER, PR2::TORQUE_CONTROL);
+ // this->myPR2->SetJointControlMode(PR2::CASTER_RL_STEER, PR2::TORQUE_CONTROL);
+ // this->myPR2->SetJointControlMode(PR2::CASTER_RR_STEER, PR2::TORQUE_CONTROL);
+
// Initialize ring buffer for point cloud data
this->cloud_pts = new ringBuffer<std_msgs::Point3DFloat32>();
this->cloud_ch1 = new ringBuffer<float>();
@@ -276,13 +280,12 @@
this->cloud_pts->allocate(this->max_cloud_pts);
this->cloud_ch1->allocate(this->max_cloud_pts);
- /*
- //-------- This doesn't seem to affect anything:
// Set control mode for the arms
- this->myPR2->SetArmControlMode(PR2::PR2_RIGHT_ARM, PR2::PR2_JOINT_CONTROL);
- this->myPR2->SetArmControlMode(PR2::PR2_LEFT_ARM, PR2::PR2_JOINT_CONTROL);
+ // FIXME: right now this just sets default to pd control
+ //this->myPR2->SetArmControlMode(PR2::PR2_RIGHT_ARM, PR2::PR2_JOINT_CONTROL);
+ //this->myPR2->SetArmControlMode(PR2::PR2_LEFT_ARM, PR2::PR2_JOINT_CONTROL);
//------------------------------------------------------------
- */
+
this->myPR2->EnableGripperLeft();
this->myPR2->EnableGripperRight();
Modified: pkg/trunk/simulators/rosgazebo/setup.bash
===================================================================
--- pkg/trunk/simulators/rosgazebo/setup.bash 2008-06-19 17:19:53 UTC (rev 857)
+++ pkg/trunk/simulators/rosgazebo/setup.bash 2008-06-19 21:42:52 UTC (rev 858)
@@ -2,8 +2,10 @@
export SIM_TOP=`rospack find gazebo`/gazebo-git
export SIM_PLUGIN=`rospack find gazebo_plugin`
export PR2API=`rospack find libpr2API`
+export PR2HW=`rospack find libprHW`
export LD_LIBRARY_PATH=$PR2API/lib:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=$PR2HW/lib:$LD_LIBRARY_PATH
export LD_LIBRARY_PATH=$SIM_PLUGIN/lib:$LD_LIBRARY_PATH
export LD_LIBRARY_PATH=$SIM_TOP/root/lib:$LD_LIBRARY_PATH
export PATH=$SIM_TOP/root/bin:$PATH
Modified: pkg/trunk/simulators/rosgazebo/setup.tcsh
===================================================================
--- pkg/trunk/simulators/rosgazebo/setup.tcsh 2008-06-19 17:19:53 UTC (rev 857)
+++ pkg/trunk/simulators/rosgazebo/setup.tcsh 2008-06-19 21:42:52 UTC (rev 858)
@@ -2,9 +2,11 @@
setenv SIM_TOP `rospack find gazebo`/gazebo-git
setenv SIM_PLUGIN `rospack find gazebo_plugin`
setenv PR2API `rospack find libpr2API`
+setenv PR2HW `rospack find libpr2HW`
if (! $?LD_LIBRARY_PATH) setenv LD_LIBRARY_PATH ''
setenv LD_LIBRARY_PATH $PR2API/lib:$LD_LIBRARY_PATH
+setenv LD_LIBRARY_PATH $PR2HW/lib:$LD_LIBRARY_PATH
setenv LD_LIBRARY_PATH $SIM_PLUGIN/lib:$LD_LIBRARY_PATH
setenv LD_LIBRARY_PATH $SIM_TOP/root/lib:$LD_LIBRARY_PATH
setenv PATH $SIM_TOP/root/bin:$PATH
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
