Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [1362] pkg/trunk
|
From: <hsu...@us...> - 2008-07-09 17:16:08
|
Revision: 1362
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1362&view=rev
Author: hsujohnhsu
Date: 2008-07-09 10:16:16 -0700 (Wed, 09 Jul 2008)
Log Message:
-----------
broadcast arm frame transforms.
only pr2.model is updated, will update pr2_torque.model next.
Modified Paths:
--------------
pkg/trunk/controllers/pr2Controllers/Makefile
pkg/trunk/controllers/pr2Controllers/manifest.xml
pkg/trunk/demos/2dnav-gazebo/world/pr2.model
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
pkg/trunk/simulators/pr2SimRT/manifest.xml
pkg/trunk/simulators/rosgazebo/rosgazebo.cc
Modified: pkg/trunk/controllers/pr2Controllers/Makefile
===================================================================
--- pkg/trunk/controllers/pr2Controllers/Makefile 2008-07-09 17:13:22 UTC (rev 1361)
+++ pkg/trunk/controllers/pr2Controllers/Makefile 2008-07-09 17:16:16 UTC (rev 1362)
@@ -1,5 +1,5 @@
SRC = src/BaseController.cpp src/HeadController.cpp src/SpineController.cpp src/ArmController.cpp src/LaserScannerController.cpp src/GripperController.cpp
-OUT = lib/libPr2Controllers.a
+OUT = lib/libpr2Controllers.a
PKG = pr2Controllers
include $(shell rospack find mk)/lib.mk
Modified: pkg/trunk/controllers/pr2Controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2Controllers/manifest.xml 2008-07-09 17:13:22 UTC (rev 1361)
+++ pkg/trunk/controllers/pr2Controllers/manifest.xml 2008-07-09 17:16:16 UTC (rev 1362)
@@ -13,6 +13,6 @@
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
<export>
- <cpp cflags='-I${prefix}/include' lflags='-L${prefix}/lib -lPr2Controllers'/>
+ <cpp cflags='-I${prefix}/include' lflags='-L${prefix}/lib -lpr2Controllers'/>
</export>
</package>
Modified: pkg/trunk/demos/2dnav-gazebo/world/pr2.model
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/world/pr2.model 2008-07-09 17:13:22 UTC (rev 1361)
+++ pkg/trunk/demos/2dnav-gazebo/world/pr2.model 2008-07-09 17:16:16 UTC (rev 1362)
@@ -179,16 +179,18 @@
<!-- neck body -->
<canonicalBody>neck_body</canonicalBody>
<body:box name="neck_body">
- <xyz>0.10 0.0 1.08</xyz>
+ <xyz>0.10 0.0 1.03</xyz>
<rpy>0.0 0.0 0.0</rpy>
<!--xyz>0.0 0.0 0.0</xyz>
<rpy>0.0 0.0 0.0</rpy-->
<geom:box name="neck_geom">
- <size>.20 .38 .14</size>
+ <!--size>.20 .38 .14</size-->
+ <size>0.01 0.01 0.01</size>
<mass>0.01</mass>
<visual>
<mesh>unit_box</mesh>
- <size>.20 .38 .14</size>
+ <!--size>.20 .38 .14</size-->
+ <size>0.01 0.01 0.01</size>
<material>Gazebo/Black</material>
</visual>
</geom:box>
@@ -221,7 +223,7 @@
<canonicalBody>head_base_body</canonicalBody>
<body:box name="head_base_body">
- <xyz>0.10 0.0 1.18 </xyz>
+ <xyz>0.10 0.0 1.13 </xyz>
<rpy>0.0 0.0 0.0 </rpy>
<!-- report when this body collides with static bodies -->
<reportStaticCollision>true</reportStaticCollision>
@@ -242,7 +244,7 @@
<canonicalBody>head_tilt_body</canonicalBody>
<body:box name="head_tilt_body">
- <xyz>0.10 0.0 1.12 </xyz>
+ <xyz>0.10 0.0 1.07 </xyz>
<rpy>0.0 0.0 0.0 </rpy>
<!-- report when this body collides with static bodies -->
<reportStaticCollision>true</reportStaticCollision>
@@ -306,7 +308,7 @@
<!-- left camera -->
<body:box name="head_cam_left_body">
- <xyz> 0.15 0.23 1.12 </xyz>
+ <xyz> 0.15 0.23 1.07 </xyz>
<rpy> 0.00 0.0 0.00 </rpy>
<sensor:camera name="head_cam_left_sensor">
<imageSize>640 480</imageSize>
@@ -333,7 +335,7 @@
</geom:box>
</body:box>
<body:box name="head_cam_base_left_body">
- <xyz> 0.15 0.20 1.12 </xyz>
+ <xyz> 0.15 0.20 1.07 </xyz>
<rpy> 0.00 0.0 0.00 </rpy>
<geom:box name="head_cam_base_left_geom">
<xyz> 0.00 0.00 0.00</xyz>
@@ -351,7 +353,7 @@
<!-- right camera -->
<body:empty name="head_cam_right_body">
- <xyz> 0.15 -0.23 1.12 </xyz>
+ <xyz> 0.15 -0.23 1.07 </xyz>
<rpy> 0.00 0.0 0.00 </rpy>
<sensor:camera name="head_cam_right_sensor">
<imageSize>640 480</imageSize>
@@ -378,7 +380,7 @@
</geom:box>
</body:empty>
<body:box name="head_cam_base_right_body">
- <xyz> 0.15 -0.20 1.12 </xyz>
+ <xyz> 0.15 -0.20 1.07 </xyz>
<rpy> 0.00 0.0 0.00 </rpy>
<geom:box name="head_cam_base_right_geom">
<xyz> 0.00 0.00 0.00</xyz>
@@ -423,7 +425,7 @@
<!-- stereo camera -->
<body:empty name="stereo_camera_body">
- <xyz> 0.10 0.0 1.3 </xyz>
+ <xyz> 0.10 0.0 1.25 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<sensor:stereocamera name="stereo_camera_sensor">
<imageSize>640 480</imageSize>
@@ -525,7 +527,7 @@
<canonicalBody>torso_body</canonicalBody>
<body:box name="torso_body">
- <xyz>0.056 0.0 0.5</xyz>
+ <xyz>0.056 0.0 0.45</xyz>
<rpy>0.0 0.0 0.0</rpy>
<!-- report when this body collides with static bodies -->
@@ -533,7 +535,7 @@
<geom:box name="torso_geom">
<xyz> 0.0 0.0 0.0 </xyz>
- <size>0.1 0.2 0.8 </size>
+ <size>0.1 0.2 0.6 </size>
<mass>1</mass>
<visual>
<!-- x y z -->
@@ -547,7 +549,7 @@
</geom:box>
<geom:box name="shoulder_geom">
- <xyz> 0.1 0.00 0.51</xyz>
+ <xyz> 0.1 0.00 0.54</xyz>
<size>0.3 0.65 0.02</size>
<mass>1</mass>
<visual>
@@ -571,7 +573,7 @@
<!-- ========== Right Arm =========== -->
<body:cylinder name="shoulder_pan_body_right">
- <xyz> 0.185 -0.15 0.68 </xyz>
+ <xyz> 0.1829361 -0.1329361 0.64 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:cylinder name="shoulder_pan_body_right_geom">
@@ -589,9 +591,9 @@
<kp>1000000.0</kp>
<kd>1.0</kd>
- <size> 0.13 0.6</size> <!-- radius and length -->
+ <size> 0.13 0.60</size> <!-- radius and length -->
<visual>
- <xyz> 0.00 0.0 0.20 </xyz>
+ <xyz> 0.00 0.0 0.185 </xyz>
<rpy> 90.0 0.0 90.0 </rpy>
<!--size> 0.26 0.6 0.36</size-->
<scale>0.001 0.001 0.001</scale>
@@ -602,7 +604,7 @@
</body:cylinder>
<body:cylinder name="upperarm_joint_right">
- <xyz> 0.285 -0.15 0.86 </xyz>
+ <xyz> 0.285 -0.15 0.8269 </xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="upperarm_joint_right_geom">
@@ -632,7 +634,7 @@
</body:cylinder>
<body:box name="upperarm_right">
- <xyz> 0.575 -0.15 0.86 </xyz>
+ <xyz> 0.575 -0.15 0.8269 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="upperarm_right_geom">
@@ -662,7 +664,7 @@
</body:box>
<body:cylinder name="elbow_right">
- <xyz> 0.6850 -0.15 0.86 </xyz>
+ <xyz> 0.6850 -0.15 0.8269 </xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="elbow_right_geom">
@@ -692,7 +694,7 @@
</body:cylinder>
<body:box name="forearm_right">
- <xyz> 0.90225 -0.15 0.86 </xyz>
+ <xyz> 0.90225 -0.15 0.8269 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="forearm_right_geom">
@@ -722,7 +724,7 @@
</body:box>
<body:cylinder name="wrist_right">
- <xyz> 1.008 -0.15 0.86 </xyz>
+ <xyz> 1.008 -0.15 0.8269 </xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="wrist_right_geom">
@@ -752,7 +754,7 @@
</body:cylinder>
<body:box name="palm_right">
- <xyz> 1.10 -0.15 0.86</xyz>
+ <xyz> 1.10 -0.15 0.8269</xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="palm_right_geom">
@@ -782,7 +784,7 @@
</body:box>
<body:box name="finger_1_right">
- <xyz> 1.18 -0.17 0.86 </xyz>
+ <xyz> 1.18 -0.17 0.8269 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="finger_1_right_geom">
<kp>100000000.0</kp>
@@ -798,7 +800,7 @@
</body:box>
<body:box name="finger_2_right">
- <xyz> 1.18 -0.13 0.86 </xyz>
+ <xyz> 1.18 -0.13 0.8269 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="finger_2_right_geom">
<kp>100000000.0</kp>
@@ -888,7 +890,7 @@
<!-- ========== Left Arm =========== -->
<body:cylinder name="shoulder_pan_body_left">
- <xyz> 0.185 0.15 0.68 </xyz>
+ <xyz> 0.1829361 0.1329361 0.64 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:cylinder name="shoulder_pan_body_left_geom">
@@ -908,7 +910,7 @@
<kd>1.0</kd>
<size> 0.13 0.6</size> <!-- radius and length -->
<visual>
- <xyz> 0.00 0.0 0.20 </xyz>
+ <xyz> 0.00 0.0 0.185 </xyz>
<rpy> 90.0 0.0 90.0 </rpy>
<!--size> 0.26 0.6 0.36</size-->
<scale>0.001 0.001 0.001</scale>
@@ -919,7 +921,7 @@
</body:cylinder>
<body:cylinder name="upperarm_joint_left">
- <xyz> 0.285 0.15 0.86 </xyz>
+ <xyz> 0.285 0.15 0.8269 </xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="upperarm_joint_left_geom">
@@ -949,7 +951,7 @@
</body:cylinder>
<body:box name="upperarm_left">
- <xyz> 0.575 0.15 0.86 </xyz>
+ <xyz> 0.575 0.15 0.8269 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="upperarm_left_geom">
@@ -979,7 +981,7 @@
</body:box>
<body:cylinder name="elbow_left">
- <xyz> 0.6850 0.15 0.86 </xyz>
+ <xyz> 0.6850 0.15 0.8269 </xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="elbow_left_geom">
@@ -1009,7 +1011,7 @@
</body:cylinder>
<body:box name="forearm_left">
- <xyz> 0.90225 0.15 0.86 </xyz>
+ <xyz> 0.90225 0.15 0.8269 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="forearm_left_geom">
@@ -1039,7 +1041,7 @@
</body:box>
<body:cylinder name="wrist_left">
- <xyz> 1.008 0.15 0.86 </xyz>
+ <xyz> 1.008 0.15 0.8269 </xyz>
<rpy> 90.0 0.0 0.0 </rpy>
<geom:cylinder name="wrist_left_geom">
@@ -1069,7 +1071,7 @@
</body:cylinder>
<body:box name="palm_left">
- <xyz> 1.10 0.15 0.86</xyz>
+ <xyz> 1.10 0.15 0.8269</xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="palm_left_geom">
@@ -1099,7 +1101,7 @@
</body:box>
<body:box name="finger_1_left">
- <xyz> 1.18 0.13 0.86 </xyz>
+ <xyz> 1.18 0.13 0.8269 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="finger_1_left_geom">
<kp>100000000.0</kp>
@@ -1115,7 +1117,7 @@
</body:box>
<body:box name="finger_2_left">
- <xyz> 1.18 0.17 0.86 </xyz>
+ <xyz> 1.18 0.17 0.8269 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
<geom:box name="finger_2_left_geom">
<kp>100000000.0</kp>
@@ -1693,7 +1695,7 @@
<!-- Hokuyo laser body -->
<canonicalBody>laser_body</canonicalBody>
<body:box name="laser_body">
- <xyz>0.23 0.0 1.05</xyz>
+ <xyz>0.23 0.0 1.0</xyz>
<rpy>0.0 0.0 0.0</rpy>
<geom:box name="laser_box">
Modified: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-09 17:13:22 UTC (rev 1361)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-09 17:16:16 UTC (rev 1362)
@@ -6,6 +6,46 @@
namespace PR2
{
+ enum PR2_FRAMEID{
+ FRAMEID_CASTER_FL_WHEEL_L = 101 , // FIXME: this is a hack until we have a frameid server
+ FRAMEID_CASTER_FL_WHEEL_R ,
+ FRAMEID_CASTER_FL_BODY ,
+ FRAMEID_CASTER_FR_WHEEL_L ,
+ FRAMEID_CASTER_FR_WHEEL_R ,
+ FRAMEID_CASTER_FR_BODY ,
+ FRAMEID_CASTER_RL_WHEEL_L ,
+ FRAMEID_CASTER_RL_WHEEL_R ,
+ FRAMEID_CASTER_RL_BODY ,
+ FRAMEID_CASTER_RR_WHEEL_L ,
+ FRAMEID_CASTER_RR_WHEEL_R ,
+ FRAMEID_CASTER_RR_BODY ,
+ FRAMEID_BASE ,
+ FRAMEID_TORSO ,
+ FRAMEID_ARM_L_TURRET ,
+ FRAMEID_ARM_L_SHOULDER ,
+ FRAMEID_ARM_L_UPPERARM ,
+ FRAMEID_ARM_L_ELBOW ,
+ FRAMEID_ARM_L_FOREARM ,
+ FRAMEID_ARM_L_WRIST ,
+ FRAMEID_ARM_L_HAND ,
+ FRAMEID_ARM_L_FINGER_1 ,
+ FRAMEID_ARM_L_FINGER_2 ,
+ FRAMEID_ARM_R_TURRET ,
+ FRAMEID_ARM_R_SHOULDER ,
+ FRAMEID_ARM_R_UPPERARM ,
+ FRAMEID_ARM_R_ELBOW ,
+ FRAMEID_ARM_R_FOREARM ,
+ FRAMEID_ARM_R_WRIST ,
+ FRAMEID_ARM_R_HAND ,
+ FRAMEID_ARM_R_FINGER_1 ,
+ FRAMEID_ARM_R_FINGER_2 ,
+ FRAMEID_HEAD_BASE ,
+ FRAMEID_LASER_BLOCK ,
+ FRAMEID_STEREO_BLOCK ,
+ FRAMEID_LASERBLOCK ,
+ MAX_FRAMEIDS
+ };
+
enum PR2_JOINT_CONTROL_MODE{
TORQUE_CONTROL,
PD_CONTROL,
@@ -100,7 +140,7 @@
ARM_L_ELBOW ,
ARM_L_FOREARM ,
ARM_L_WRIST ,
- ARM_L_GRIPPER_TMP ,
+ ARM_L_HAND ,
ARM_L_FINGER_1 ,
ARM_L_FINGER_2 ,
ARM_R_TURRET ,
@@ -109,7 +149,7 @@
ARM_R_ELBOW ,
ARM_R_FOREARM ,
ARM_R_WRIST ,
- ARM_R_GRIPPER_TMP ,
+ ARM_R_HAND ,
ARM_R_FINGER_1 ,
ARM_R_FINGER_2 ,
HEAD_BASE ,
@@ -243,7 +283,7 @@
const int JointEnd[MAX_MODELS] = {CASTER_RR_DRIVE_R ,SPINE_ELEVATOR ,ARM_L_WRIST_ROLL ,ARM_R_WRIST_ROLL ,ARM_L_GRIPPER ,ARM_R_GRIPPER ,HEAD_LASER_PITCH };
-// Geometric description for the base
+ // Geometric description for the base
const float CASTER_FL_X_OFFSET = 0.25;
const float CASTER_FL_Y_OFFSET = 0.25;
Modified: pkg/trunk/simulators/pr2SimRT/manifest.xml
===================================================================
--- pkg/trunk/simulators/pr2SimRT/manifest.xml 2008-07-09 17:13:22 UTC (rev 1361)
+++ pkg/trunk/simulators/pr2SimRT/manifest.xml 2008-07-09 17:16:16 UTC (rev 1362)
@@ -7,7 +7,7 @@
<author>John M. Hsu</author>
<license>TBD</license>
<depend package="roscpp" />
- <depend package="libpr2GZ" />
+ <depend package="libpr2API" />
<depend package="genericControllers" />
<depend package="pr2Controllers" />
<depend package="pr2Core" />
Modified: pkg/trunk/simulators/rosgazebo/rosgazebo.cc
===================================================================
--- pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-07-09 17:13:22 UTC (rev 1361)
+++ pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-07-09 17:16:16 UTC (rev 1362)
@@ -505,11 +505,11 @@
this->odomMsg.vel.th = vw;
// Get position
- double x,y,z,roll,pitch,th;
- this->myPR2->GetBasePositionActual(&x,&y,&z,&roll,&pitch,&th);
+ double x,y,z,roll,pitch,yaw;
+ this->myPR2->GetBasePositionActual(&x,&y,&z,&roll,&pitch,&yaw);
this->odomMsg.pos.x = x;
this->odomMsg.pos.y = y;
- this->odomMsg.pos.th = th;
+ this->odomMsg.pos.th = yaw;
// this->odomMsg.stall = this->positionmodel->Stall();
// TODO: get the frame ID from somewhere
@@ -667,7 +667,211 @@
// this->myPR2->OpenGripper(PR2::PR2_LEFT_GRIPPER ,this->arm.gripperGapCmd,this->arm.gripperForceCmd);
// this->myPR2->CloseGripper(PR2::PR2_RIGHT_GRIPPER,this->arm.gripperGapCmd,this->arm.gripperForceCmd);
+ /***************************************************************/
+ /* */
+ /* frame transforms */
+ /* */
+ /* x,y,z,yaw,pitch,roll */
+ /* */
+ /***************************************************************/
+ //double x,y,z,roll,pitch,yaw;
+ //this->myPR2->GetBasePositionActual(&x,&y,&z,&roll,&pitch,&yaw); // actual CoM of base
+ tf.sendInverseEuler(FRAMEID_ROBOT,
+ PR2::FRAMEID_BASE,
+ 0.0,
+ 0.0,
+ -0.13, /* half height of base box */
+ yaw,
+ pitch,
+ roll,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+ // base = center of the bottom of the base box
+ // torso = midpoint of bottom of turrets
+ tf.sendInverseEuler(PR2::FRAMEID_BASE,
+ PR2::FRAMEID_TORSO,
+ PR2::BASE_LEFT_ARM_OFFSET.x,
+ 0.0,
+ PR2::BASE_LEFT_ARM_OFFSET.z, /* FIXME: spine elevator not accounted for */
+ 0.0,
+ 0.0,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_l_turret = bottom of left turret
+ tf.sendInverseEuler(PR2::FRAMEID_TORSO,
+ PR2::FRAMEID_ARM_L_TURRET,
+ 0.0,
+ PR2::BASE_LEFT_ARM_OFFSET.y,
+ 0.0,
+ larm.turretAngle,
+ 0.0,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_l_shoulder = center of left shoulder pitch bracket
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_L_TURRET,
+ PR2::FRAMEID_ARM_L_SHOULDER,
+ PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.x,
+ PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.y,
+ PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.z,
+ 0.0,
+ larm.shoulderLiftAngle,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_l_upperarm = upper arm with roll DOF, at shoulder pitch center
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_L_SHOULDER,
+ PR2::FRAMEID_ARM_L_UPPERARM,
+ PR2::ARM_SHOULDER_PITCH_ROLL_OFFSET.x,
+ PR2::ARM_SHOULDER_PITCH_ROLL_OFFSET.y,
+ PR2::ARM_SHOULDER_PITCH_ROLL_OFFSET.z,
+ 0.0,
+ 0.0,
+ larm.upperarmRollAngle,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ //frameid_arm_l_elbow = elbow pitch bracket center of rotation
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_L_UPPERARM,
+ PR2::FRAMEID_ARM_L_ELBOW,
+ PR2::ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.x,
+ PR2::ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.y,
+ PR2::ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.z,
+ 0.0,
+ larm.elbowAngle,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ //frameid_arm_l_forearm = forearm roll DOR, at elbow pitch center
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_L_ELBOW,
+ PR2::FRAMEID_ARM_L_FOREARM,
+ PR2::ELBOW_PITCH_ELBOW_ROLL_OFFSET.x,
+ PR2::ELBOW_PITCH_ELBOW_ROLL_OFFSET.y,
+ PR2::ELBOW_PITCH_ELBOW_ROLL_OFFSET.z,
+ 0.0,
+ 0.0,
+ larm.forearmRollAngle,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_l_wrist = wrist pitch DOF.
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_L_FOREARM,
+ PR2::FRAMEID_ARM_L_WRIST,
+ PR2::ELBOW_ROLL_WRIST_PITCH_OFFSET.x,
+ PR2::ELBOW_ROLL_WRIST_PITCH_OFFSET.y,
+ PR2::ELBOW_ROLL_WRIST_PITCH_OFFSET.z,
+ 0.0,
+ larm.wristPitchAngle,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_l_hand = hand roll DOF, center at wrist pitch center
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_L_WRIST,
+ PR2::FRAMEID_ARM_L_HAND,
+ PR2::WRIST_PITCH_WRIST_ROLL_OFFSET.x,
+ PR2::WRIST_PITCH_WRIST_ROLL_OFFSET.y,
+ PR2::WRIST_PITCH_WRIST_ROLL_OFFSET.z,
+ 0.0,
+ 0.0,
+ larm.wristRollAngle,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+
+ // arm_r_turret = bottom of right turret
+ tf.sendInverseEuler(PR2::FRAMEID_TORSO,
+ PR2::FRAMEID_ARM_R_TURRET,
+ 0.0,
+ PR2::BASE_RIGHT_ARM_OFFSET.y,
+ 0.0,
+ rarm.turretAngle,
+ 0.0,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_r_shoulder = center of right shoulder pitch bracket
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_R_TURRET,
+ PR2::FRAMEID_ARM_R_SHOULDER,
+ PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.x,
+ PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.y,
+ PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.z,
+ 0.0,
+ rarm.shoulderLiftAngle,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_r_upperarm = upper arm with roll DOF, at shoulder pitch center
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_R_SHOULDER,
+ PR2::FRAMEID_ARM_R_UPPERARM,
+ PR2::ARM_SHOULDER_PITCH_ROLL_OFFSET.x,
+ PR2::ARM_SHOULDER_PITCH_ROLL_OFFSET.y,
+ PR2::ARM_SHOULDER_PITCH_ROLL_OFFSET.z,
+ 0.0,
+ 0.0,
+ rarm.upperarmRollAngle,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ //frameid_arm_r_elbow = elbow pitch bracket center of rotation
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_R_UPPERARM,
+ PR2::FRAMEID_ARM_R_ELBOW,
+ PR2::ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.x,
+ PR2::ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.y,
+ PR2::ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.z,
+ 0.0,
+ rarm.elbowAngle,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ //frameid_arm_r_forearm = forearm roll DOR, at elbow pitch center
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_R_ELBOW,
+ PR2::FRAMEID_ARM_R_FOREARM,
+ PR2::ELBOW_PITCH_ELBOW_ROLL_OFFSET.x,
+ PR2::ELBOW_PITCH_ELBOW_ROLL_OFFSET.y,
+ PR2::ELBOW_PITCH_ELBOW_ROLL_OFFSET.z,
+ 0.0,
+ 0.0,
+ rarm.forearmRollAngle,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_r_wrist = wrist pitch DOF.
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_R_FOREARM,
+ PR2::FRAMEID_ARM_R_WRIST,
+ PR2::ELBOW_ROLL_WRIST_PITCH_OFFSET.x,
+ PR2::ELBOW_ROLL_WRIST_PITCH_OFFSET.y,
+ PR2::ELBOW_ROLL_WRIST_PITCH_OFFSET.z,
+ 0.0,
+ rarm.wristPitchAngle,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+ // arm_r_hand = hand roll DOF, center at wrist pitch center
+ tf.sendInverseEuler(PR2::FRAMEID_ARM_R_WRIST,
+ PR2::FRAMEID_ARM_R_HAND,
+ PR2::WRIST_PITCH_WRIST_ROLL_OFFSET.x,
+ PR2::WRIST_PITCH_WRIST_ROLL_OFFSET.y,
+ PR2::WRIST_PITCH_WRIST_ROLL_OFFSET.z,
+ 0.0,
+ 0.0,
+ rarm.wristRollAngle,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
+
+
+
this->lock.unlock();
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
