Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2837] pkg/trunk
|
From: <adv...@us...> - 2008-08-08 22:35:57
|
Revision: 2837
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2837&view=rev
Author: advaitjain
Date: 2008-08-08 22:35:59 +0000 (Fri, 08 Aug 2008)
Log Message:
-----------
* kinematics now works from robot_kinematics. will soon move libKDL to deprecated.
* working object grasping demo in simulation.
* changed manifest.xml of rosgazebo, pr2_gazebo to depend on robot_kinematics instead of libKDL.
* pr2_gazebo - contoller initialisation has been commented.
* added pr2.xml.nolimits -- for use with object grasping demo.
Modified Paths:
--------------
pkg/trunk/clean_rosgazebo.scp
pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/manip/overhead_grasp_behavior/manifest.xml
pkg/trunk/manip/overhead_grasp_behavior/src/overhead_grasp.cc
pkg/trunk/manip/pr2_kinematic_controllers/manifest.xml
pkg/trunk/manip/pr2_kinematic_controllers/src/pr2_kinematic_controllers.cc
pkg/trunk/manip/pr2_kinematic_controllers/test/test_pr2_kin_con.cc
pkg/trunk/manip/teleop_arm_keyboard/Makefile
pkg/trunk/robot_control_loops/pr2_gazebo/manifest.xml
pkg/trunk/robot_control_loops/pr2_gazebo/src/pr2_gazebo.cpp
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_tablegrasp.world
pkg/trunk/simulators/rosgazebo/CMakeLists.txt
pkg/trunk/simulators/rosgazebo/manifest.xml
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
pkg/trunk/util/kinematics/libKDL/src/kdl_kinematics.cpp
pkg/trunk/util/kinematics/libKDL/test/pr2_kin_test.cpp
Added Paths:
-----------
pkg/trunk/drivers/robot/pr2/libpr2API/src/old/kinematic_controllers.h
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml.nolimits
Removed Paths:
-------------
pkg/trunk/drivers/robot/pr2/libpr2API/include/kinematic_controllers.h
Modified: pkg/trunk/clean_rosgazebo.scp
===================================================================
--- pkg/trunk/clean_rosgazebo.scp 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/clean_rosgazebo.scp 2008-08-08 22:35:59 UTC (rev 2837)
@@ -29,6 +29,7 @@
# clean kinematics
(cd util/kinematics/libKinematics ; rm -f lib/* ;make clean)
(cd util/kinematics/libKDL ; rm -f lib/* ;make clean)
+(cd util/kinematics/robot_kinematics ; rm -f lib/* ;make clean)
# clean visualization
(cd visualization/irrlicht_viewer ; rm -f lib/* ;make clean)
Deleted: pkg/trunk/drivers/robot/pr2/libpr2API/include/kinematic_controllers.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/kinematic_controllers.h 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/kinematic_controllers.h 2008-08-08 22:35:59 UTC (rev 2837)
@@ -1,25 +0,0 @@
-#ifndef KINEMATIC_CONTROLLERS_H
-#define KINEMATIC_CONTROLLERS_H
-
-#include <libpr2API/pr2API.h>
-
-class kinematic_controllers
-{
- public:
- PR2::PR2Robot* myPR2;
-
- public:
- kinematic_controllers();
-
- // linear interpolation between current and desired position.
- // p,r -- desired pose, class stores current joint angles.
- void go(KDL::Vector p, KDL::Rotation r, double step_size);
- void init();
-
-
-};
-
-
-
-#endif
-
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-08-08 22:35:59 UTC (rev 2837)
@@ -41,7 +41,9 @@
#include <libpr2HW/pr2HW.h>
-#include <libKDL/kdl_kinematics.h> // for kinematics using KDL -- util/kinematics/libKDL
+//#include <libKDL/kdl_kinematics.h> // for kinematics using KDL -- util/kinematics/libKDL
+#include <robot_kinematics/serial_chain.h>
+#include <robot_kinematics/robot_kinematics.h>
namespace PR2
{
@@ -618,7 +620,8 @@
protected: PR2Arm myArm;
protected: PR2Head myHead;
- public: PR2_kinematics pr2_kin; // for kinematics using KDL.
+ public: robot_kinematics::SerialChain *right_arm_chain_;
+ public: robot_kinematics::RobotKinematics pr2_kin;
};
}
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml 2008-08-08 22:35:59 UTC (rev 2837)
@@ -11,7 +11,8 @@
<depend package="pr2Core"/>
<depend package="libpr2HW"/>
<depend package="libKinematics"/>
-<depend package="libKDL"/>
+<!-- <depend package="libKDL"/> -->
+<depend package="robot_kinematics"/>
<depend package="rosTF"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lpr2API"/>
Copied: pkg/trunk/drivers/robot/pr2/libpr2API/src/old/kinematic_controllers.h (from rev 2663, pkg/trunk/drivers/robot/pr2/libpr2API/include/kinematic_controllers.h)
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/old/kinematic_controllers.h (rev 0)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/old/kinematic_controllers.h 2008-08-08 22:35:59 UTC (rev 2837)
@@ -0,0 +1,25 @@
+#ifndef KINEMATIC_CONTROLLERS_H
+#define KINEMATIC_CONTROLLERS_H
+
+#include <libpr2API/pr2API.h>
+
+class kinematic_controllers
+{
+ public:
+ PR2::PR2Robot* myPR2;
+
+ public:
+ kinematic_controllers();
+
+ // linear interpolation between current and desired position.
+ // p,r -- desired pose, class stores current joint angles.
+ void go(KDL::Vector p, KDL::Rotation r, double step_size);
+ void init();
+
+
+};
+
+
+
+#endif
+
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-08-08 22:35:59 UTC (rev 2837)
@@ -3,7 +3,10 @@
#include <pr2Core/pr2Misc.h>
#include <math.h>
+#include <robot_kinematics/robot_kinematics.h>
+
using namespace PR2;
+using namespace robot_kinematics;
PR2Robot::PR2Robot()
{
@@ -15,8 +18,16 @@
PR2_ERROR_CODE PR2Robot::InitializeRobot()
{
// initialize robot
- hw.Init();
- return PR2_ALL_OK;
+ hw.Init();
+
+ //------- what is the best way to initialise the kinematics? -------
+ char *c_filename = getenv("ROS_PACKAGE_PATH");
+ std::stringstream filename;
+ filename << c_filename << "/robot_descriptions/wg_robot_description/pr2/pr2.xml" ;
+
+ pr2_kin.loadXML(filename.str());
+ right_arm_chain_ = pr2_kin.getSerialChain("rightArm");
+ return PR2_ALL_OK;
}
PR2_ERROR_CODE PR2Robot::CalibrateRobot()
@@ -99,7 +110,7 @@
*/
PR2_ERROR_CODE PR2Robot::SetArmCartesianPosition(PR2_MODEL_ID id, const KDL::Frame &f, const KDL::JntArray &q_init, KDL::JntArray &q_out)
{
- if (this->pr2_kin.IK(f) == false)
+ if (this->right_arm_chain_->computeIK(f) == false)
{
printf("[libpr2API]<pr2API.cpp> Could not compute Inv Kin.\n");
return PR2_ALL_OK;
@@ -113,10 +124,11 @@
/*
* This is the function which should be used -- Advait
+ * only right_chain is currently handled.
*/
PR2_ERROR_CODE PR2Robot::SetArmCartesianPosition(PR2_MODEL_ID id, const KDL::Frame &f, bool &reachable)
{
- reachable = this->pr2_kin.IK(f);
+ reachable = this->right_arm_chain_->computeIK(f);
if (reachable == false)
{
printf("[libpr2API]<pr2API.cpp> Could not compute Inv Kin.\n");
@@ -124,11 +136,11 @@
}
for(int ii = 0; ii < 7; ii++)
- hw.SetJointServoCmd((PR2::PR2_JOINT_ID) (JointStart[id]+ii),(*this->pr2_kin.q_IK_result)(ii),0);
+ hw.SetJointServoCmd((PR2::PR2_JOINT_ID) (JointStart[id]+ii),(*this->right_arm_chain_->q_IK_result)(ii),0);
- KDL::JntArray *t = this->pr2_kin.q_IK_result;
- this->pr2_kin.q_IK_result = this->pr2_kin.q_IK_guess;
- this->pr2_kin.q_IK_guess = t; // update guess with the computed IK result.
+ KDL::JntArray *t = this->right_arm_chain_->q_IK_result;
+ this->right_arm_chain_->q_IK_result = this->right_arm_chain_->q_IK_guess;
+ this->right_arm_chain_->q_IK_guess = t; // update guess with the computed IK result.
return PR2_ALL_OK;
@@ -544,7 +556,7 @@
return PR2_ERROR;
double pos,vel;
- for(int ii = JointStart[id]; ii <= JointEnd[id]; ii++)
+ for(int ii = JointStart[id]; ii < JointEnd[id]; ii++) // KDL does not use the gripper and doesn't need the gripper joint angle.
{
hw.GetJointServoCmd((PR2_JOINT_ID)ii,&pos,&vel);
q(ii-JointStart[id]) = pos;
Modified: pkg/trunk/manip/overhead_grasp_behavior/manifest.xml
===================================================================
--- pkg/trunk/manip/overhead_grasp_behavior/manifest.xml 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/manip/overhead_grasp_behavior/manifest.xml 2008-08-08 22:35:59 UTC (rev 2837)
@@ -8,7 +8,6 @@
<depend package="std_msgs"/>
<depend package="pr2Core"/>
<depend package="libpr2API"/>
-<depend package="libKDL"/>
<depend package="pr2_gazebo"/>
-<depend package="interpolated_kinematic_controller"/>
+<depend package="pr2_kinematic_controllers"/>
</package>
Modified: pkg/trunk/manip/overhead_grasp_behavior/src/overhead_grasp.cc
===================================================================
--- pkg/trunk/manip/overhead_grasp_behavior/src/overhead_grasp.cc 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/manip/overhead_grasp_behavior/src/overhead_grasp.cc 2008-08-08 22:35:59 UTC (rev 2837)
@@ -6,14 +6,18 @@
#include <libpr2API/pr2API.h>
#include <pr2_msgs/EndEffectorState.h>
+#include <pr2_kinematic_controllers/Float64Int32.h>
+
#include <std_msgs/Point3DFloat32.h>
#include <std_msgs/Float64.h>
#include <std_msgs/PR2Arm.h>
#include <unistd.h>
-using namespace KDL;
+#include <rosgazebo/GripperCmd.h>
+#include <std_msgs/PR2Arm.h>
+using namespace KDL;
class OverheadGrasper : public ros::node
{
@@ -21,38 +25,68 @@
// coordinates of arm's origin in gazebo/arm coordinate frame (the axes of both frames are parallel)
Vector gazebo_to_arm_vector;
std_msgs::Point3DFloat32 objectPosMsg;
+ pr2_msgs::EndEffectorState rightEndEffectorMsg;
+ Frame right_tooltip_frame;
+ Vector objectPosition;
+ const static double PR2_GRIPPER_LENGTH = 0.155;
+ Vector CAMERA_ENDEFFECTOR; // position of camera relative to end effector in end effector frame.
public:
OverheadGrasper(void) : ros::node("overhead_grasper")
{
advertise<std_msgs::PR2Arm>("cmd_rightarmconfig");
- advertise<pr2_msgs::EndEffectorState>("right_pr2arm_set_end_effector");
+ advertise<pr2_msgs::EndEffectorState>("right_pr2arm_set_end_effector"); // this should actually be a service.
advertise<std_msgs::Float64>("interpolate_step_size");
advertise<std_msgs::Float64>("interpolate_wait_time");
subscribe("object_position", objectPosMsg, &OverheadGrasper::objectPosition_cb);
+ subscribe("rightarm_tooltip_cartesian", rightEndEffectorMsg, &OverheadGrasper::currentRightArmPosCartesian_cb);
- gazebo_to_arm_vector = Vector(1.040-0.7987,-0.15,0.8269);
+// gazebo_to_arm_vector = Vector(1.020-0.7987,-0.15,0.8269);
+ gazebo_to_arm_vector = Vector(0.81-0.82025,-0.20,0.739675);
+
+ CAMERA_ENDEFFECTOR = Vector(0.0, 0.,0.05); // position of camera relative to end effector in base frame.
}
- void positionArmCartesian(Vector v, Rotation r)
+ void KDL_to_EndEffectorStateMsg(const Frame& f, pr2_msgs::EndEffectorState &efs)
{
- pr2_msgs::EndEffectorState efs;
efs.set_rot_size(9);
efs.set_trans_size(3);
- for(int i = 0; i < 9; i++) {
- efs.rot[i] = r.data[i];
- }
- for(int i = 0; i < 3; i++) {
- efs.trans[i] = v.data[i];
- }
+ for(int i = 0; i < 9; i++)
+ efs.rot[i] = f.M.data[i];
+ for(int i = 0; i < 3; i++)
+ efs.trans[i] = f.p.data[i];
+ }
+ void EndEffectorStateMsg_to_KDL(const pr2_msgs::EndEffectorState &efs, Frame& f)
+ {
+ for(int i = 0; i < 9; i++)
+ f.M.data[i] = efs.rot[i];
+ for(int i = 0; i < 3; i++)
+ f.p.data[i] = efs.trans[i];
+ }
+
+ void positionArmCartesian(const Vector& v, const Rotation& r)
+ {
+ pr2_msgs::EndEffectorState efs;
+ Frame f(r,v);
+ KDL_to_EndEffectorStateMsg(f,efs);
+
publish("right_pr2arm_set_end_effector",efs);
}
-
void overheadGraspPosture()
{
std_msgs::PR2Arm rightarm;
+// rightarm.turretAngle = deg2rad*0;
+// rightarm.shoulderLiftAngle = deg2rad*0;
+// rightarm.upperarmRollAngle = deg2rad*0;
+// rightarm.elbowAngle = deg2rad*0;
+// rightarm.forearmRollAngle = 0;
+// rightarm.wristPitchAngle = deg2rad*90;
+// rightarm.wristRollAngle = 0;
+// rightarm.gripperForceCmd = 50;
+// rightarm.gripperGapCmd = 0.;
+
rightarm.turretAngle = deg2rad*-20;
rightarm.shoulderLiftAngle = deg2rad*-20;
rightarm.upperarmRollAngle = deg2rad*180;
@@ -60,27 +94,103 @@
rightarm.forearmRollAngle = 0;
rightarm.wristPitchAngle = 0;
rightarm.wristRollAngle = 0;
- rightarm.gripperForceCmd = 0;
- rightarm.gripperGapCmd = 0.2;
+ rightarm.gripperForceCmd = 50;
+ rightarm.gripperGapCmd = 0.;
publish("cmd_rightarmconfig",rightarm);
}
-
- // Vector v is in gazebo coordinate frame.
- void positionEyecamOverObject(Vector v)
+ // Vector v is shoulder coordinate frame.
+// void positionEyecamOverObject(Vector v)
+ void positionEyecamOverObject()
{
- Vector v_arm = v - gazebo_to_arm_vector;
- v_arm.data[2] += 0.4; // I want end effector to be 0.5m above object.
- Rotation r = Rotation::RotY(DTOR(90)); // look down vertically
+ Vector v_arm = objectPosition;
+ v_arm.data[2] += 0.3; // I want end effector to be 0.4m above object.
+ Rotation r = Rotation::RotY(deg2rad*90) * Rotation::RotY(deg2rad*90); // look down vertically
cout<<"Going to: "<<v_arm<<endl;
positionArmCartesian(v_arm, r);
}
+ void moveArmSegmentation()
+ {
+// gmmseg::hrl_grasp:request req;
+// gmmseg::hrl_grasp:response res;
+// req.height = right_tooltip_frame.p[2] - objectPosition[2] - CAMERA_ENDEFFECTOR[0];
+// if (ros::service::call("hrl_grasp", req, res)==false)
+// {
+// printf("[overhead_grasp_behavior] <overhead_grasp.cpp> {segmentObject} hrl_grasp service failed.\nExiting..\n");
+// exit(0);
+// }
+// Vector move(-1*res.y, -1*res.x, 0);
+// Rotation r = Rotation::RotY(deg2rad*90)*Rotation::RotX(res.theta); // look down vertically, with correct wrist roll
+
+ Vector move(0,0,0);
+ move += Rotation::RotY(deg2rad*90)*CAMERA_ENDEFFECTOR;
+ Rotation r = Rotation::RotY(deg2rad*90)*Rotation::RotY(deg2rad*90); // look down vertically, with correct wrist roll
+ Vector goto_point = right_tooltip_frame.p+move;
+ cout<<"Going to: "<<goto_point<<endl;
+ positionArmCartesian(goto_point, r);
+ }
+
void objectPosition_cb(void)
{
+// cout<<"object's z coord: "<<objectPosMsg.z<<"\n";
+ objectPosition = Vector(objectPosMsg.x,objectPosMsg.y,objectPosMsg.z) - gazebo_to_arm_vector;
}
+ void currentRightArmPosCartesian_cb(void)
+ {
+ EndEffectorStateMsg_to_KDL(this->rightEndEffectorMsg, this->right_tooltip_frame);
+ }
+
+ void OpenGripper(void)
+ {
+ rosgazebo::GripperCmd::request req;
+ rosgazebo::GripperCmd::response res;
+ req.gap=0.3;
+ req.force=50;
+ if (ros::service::call("operate_right_gripper", req, res)==false)
+ {
+ printf("[overhead_grasp_behavior] <overhead_grasp.cpp> {OpenGripper} operate_right_gripper service failed.\nExiting..\n");
+ exit(0);
+ }
+ }
+
+ void CloseGripper(void)
+ {
+ rosgazebo::GripperCmd::request req;
+ rosgazebo::GripperCmd::response res;
+ req.gap=0.0;
+ req.force=200;
+ if (ros::service::call("operate_right_gripper", req, res)==false)
+ {
+ printf("[overhead_grasp_behavior] <overhead_grasp.cpp> {CloseGripper} operate_right_gripper service failed.\nExiting..\n");
+ exit(0);
+ }
+ }
+
+ void pickUpObject(void)
+ {
+ pr2_kinematic_controllers::Float64Int32::request req;
+ pr2_kinematic_controllers::Float64Int32::response res;
+ req.f = -1*PR2_GRIPPER_LENGTH;
+ ros::service::call("move_along_gripper", req, res);
+ }
+
+ void printTooltipTransformation(void)
+ {
+ cout<<"End effector transformation: "<<this->right_tooltip_frame<<"\n";
+ }
+
+ void graspFromAbove(void)
+ {
+ pr2_kinematic_controllers::Float64Int32::request req;
+ pr2_kinematic_controllers::Float64Int32::response res;
+ printf("arm tip z: %.4f\nobjectPosition.z: %.4f\n", right_tooltip_frame.p.data[2],objectPosition.data[2]);
+ req.f = right_tooltip_frame.p.data[2] - objectPosition.data[2] - PR2_GRIPPER_LENGTH;
+ printf("amount to move by: %.4f\n", req.f);
+ ros::service::call("move_along_gripper", req, res);
+ }
};
@@ -96,11 +206,11 @@
std_msgs::Float64 float64_msg;
sleep(1);
- double interpolate_step_size = 0.05;
+ double interpolate_step_size = 0.01;
float64_msg.data = interpolate_step_size;
ohGrasper.publish("interpolate_step_size", float64_msg);
- double interpolate_wait_time = 1.0;
+ double interpolate_wait_time = .3;
float64_msg.data = interpolate_wait_time;
ohGrasper.publish("interpolate_wait_time", float64_msg);
@@ -162,13 +272,42 @@
break;
case '2':
{
- Vector v(n.objectPosMsg.x,n.objectPosMsg.y,n.objectPosMsg.z);
- n.positionEyecamOverObject(v);
+ n.positionEyecamOverObject();
}
break;
case '3':
- printf("x: %.4f y: %.4f z: %.4f\n", n.objectPosMsg.x, n.objectPosMsg.y, n.objectPosMsg.z);
+ n.graspFromAbove();
break;
+ case '4':
+ n.pickUpObject();
+ break;
+
+ case 't':
+ n.moveArmSegmentation();
+ break;
+
+ case 'e':
+ n.printTooltipTransformation();
+ break;
+
+ case 's':
+ {
+ pr2_kinematic_controllers::Float64Int32::request req;
+ pr2_kinematic_controllers::Float64Int32::response res;
+ req.f=0.01;
+ ros::service::call("move_along_gripper", req, res);
+ }
+
+ // printf("x: %.4f y: %.4f z: %.4f\n", n.objectPosMsg.x, n.objectPosMsg.y, n.objectPosMsg.z);
+ break;
+ case 'o':
+ case 'O':
+ n.OpenGripper();
+ break;
+ case 'c':
+ case 'C':
+ n.CloseGripper();
+ break;
default:
break;
}
Modified: pkg/trunk/manip/pr2_kinematic_controllers/manifest.xml
===================================================================
--- pkg/trunk/manip/pr2_kinematic_controllers/manifest.xml 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/manip/pr2_kinematic_controllers/manifest.xml 2008-08-08 22:35:59 UTC (rev 2837)
@@ -9,7 +9,8 @@
<depend package="pr2_msgs"/>
<depend package="pr2Core"/>
<depend package="libpr2API"/>
- <depend package="libKDL"/>
+ <!-- <depend package="libKDL"/> -->
+ <depend package="robot_kinematics"/>
<depend package="pr2_gazebo"/>
<export>
<cpp cflags="-I${prefix}/srv/cpp" />
Modified: pkg/trunk/manip/pr2_kinematic_controllers/src/pr2_kinematic_controllers.cc
===================================================================
--- pkg/trunk/manip/pr2_kinematic_controllers/src/pr2_kinematic_controllers.cc 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/manip/pr2_kinematic_controllers/src/pr2_kinematic_controllers.cc 2008-08-08 22:35:59 UTC (rev 2837)
@@ -1,40 +1,52 @@
#include <ros/node.h>
-#include <rosthread/mutex.h>
-#include <std_msgs/Empty.h>
#include <std_msgs/PR2Arm.h>
#include <std_msgs/Float64.h>
#include <pr2_msgs/EndEffectorState.h>
+#include <pr2_kinematic_controllers/Float64Int32.h>
+#include <rosgazebo/VoidVoid.h>
+#include <rosgazebo/MoveCartesian.h>
-#include <pr2_kinematic_controllers/Float32Int32.h>
-
#include <libpr2API/pr2API.h>
+//#include <libKDL/kdl_kinematics.h>
+#include <robot_kinematics/robot_kinematics.h>
+#include <robot_kinematics/serial_chain.h>
+
#include <kdl/rotational_interpolation_sa.hpp>
-#include <rosgazebo/VoidVoid.h>
+#include <unistd.h>
using namespace std_msgs;
using namespace PR2;
using namespace KDL;
+using namespace robot_kinematics;
+
class Pr2KinematicControllers : public ros::node
{
public:
Pr2KinematicControllers(void) : ros::node("pr2_kinematic_controller")
{
+ char *c_filename = getenv("ROS_PACKAGE_PATH");
+ std::stringstream filename;
+ filename << c_filename << "/robot_descriptions/wg_robot_description/pr2/pr2.xml" ;
+ pr2_kin.loadXML(filename.str());
+ right_arm = pr2_kin.getSerialChain("rightArm");
+
step_size = 0.05;
- wait_time = 1.;
+ wait_time = 1;
advertise<pr2_msgs::EndEffectorState>("cmd_leftarm_cartesian");
- advertise<pr2_msgs::EndEffectorState>("cmd_rightarm_cartesian");
-// advertise<std_msgs::Empty>("reset_IK_guess"); // to tell RosGazeboNode to make q_IK_guess = current manipulator config.
+ advertise<pr2_msgs::EndEffectorState>("rightarm_tooltip_cartesian"); // position of tip of right arm in cartesian coord.
+ advertise_service("move_along_gripper", &Pr2KinematicControllers::moveAlongGripper);
+
subscribe("right_pr2arm_set_end_effector", _rightEndEffectorGoal, &Pr2KinematicControllers::setRightEndEffector);
subscribe("left_pr2arm_set_end_effector", _leftEndEffectorGoal, &Pr2KinematicControllers::setLeftEndEffector);
- subscribe("left_pr2arm_pos", leftArmPosMsg, &Pr2KinematicControllers::currentLeftArmPos);
- subscribe("right_pr2arm_pos", rightArmPosMsg, &Pr2KinematicControllers::currentRightArmPos);
+ subscribe("left_pr2arm_pos", leftArmPosMsg, &Pr2KinematicControllers::currentLeftArmPos); // configuration of left arm.
+ subscribe("right_pr2arm_pos", rightArmPosMsg, &Pr2KinematicControllers::currentRightArmPos); // configuration of right arm.
subscribe("interpolate_step_size", float64_msg, &Pr2KinematicControllers::setStepSize);
subscribe("interpolate_wait_time", float64_msg, &Pr2KinematicControllers::setWaitTime);
@@ -77,32 +89,42 @@
void currentLeftArmPos(void)
{
- // don't need to do anything -- we already have the data
}
void currentRightArmPos(void)
{
- // don't need to do anything -- we already have the data
+ JntArray q = JntArray(right_arm->num_joints_);
+ q(0) = rightArmPosMsg.turretAngle;
+ q(1) = rightArmPosMsg.shoulderLiftAngle;
+ q(2) = rightArmPosMsg.upperarmRollAngle;
+ q(3) = rightArmPosMsg.elbowAngle;
+ q(4) = rightArmPosMsg.forearmRollAngle;
+ q(5) = rightArmPosMsg.wristPitchAngle;
+ q(6) = rightArmPosMsg.wristRollAngle;
+
+ Frame f;
+ right_arm->computeFK(q,f);
+ pr2_msgs::EndEffectorState efs;
+ KDL_to_EndEffectorStateMsg(f, efs);
+ publish("rightarm_tooltip_cartesian",efs);
}
- void publishFrame(bool isRightArm, const Frame& f)
+ void KDL_to_EndEffectorStateMsg(const Frame& f, pr2_msgs::EndEffectorState &efs)
{
- pr2_msgs::EndEffectorState efs;
efs.set_rot_size(9);
efs.set_trans_size(3);
- std::cout << "Publishing rot ";
for(int i = 0; i < 9; i++)
- {
efs.rot[i] = f.M.data[i];
- std::cout << efs.rot[i] << " ";
- }
- std::cout << " trans ";
+
for(int i = 0; i < 3; i++)
- {
efs.trans[i] = f.p.data[i];
- std::cout << efs.trans[i] << " ";
- }
- std::cout << std::endl;
+ }
+
+ void publishFrame(bool isRightArm, const Frame& f)
+ {
+ pr2_msgs::EndEffectorState efs;
+ KDL_to_EndEffectorStateMsg(f, efs);
+
if(isRightArm)
publish("cmd_rightarm_cartesian",efs);
else
@@ -111,29 +133,18 @@
void RunControlLoop(bool isRightArm, const Frame& r, const std_msgs::PR2Arm& arm)
{
-
- std_msgs::Empty emp;
rosgazebo::VoidVoid::request req;
rosgazebo::VoidVoid::response res;
-// publish("reset_IK_guess",emp);
- if (ros::service::call("reset_IK_guess", req, res))
- printf("Success!\n");
- else
+
+ cout<<"RunControlLoop: rotation: "<<r.M<<"\n";
+
+ if (ros::service::call("reset_IK_guess", req, res)==false)
{
- printf("reset_IK_guess service failed.\nExiting..\n");
+ printf("[pr2_kinematic_controllers] <pr2_kinematic_controllers.cpp> reset_IK_guess service failed.\nExiting..\n");
exit(0);
}
-// if (ros::service::call("reset_IK_guess", req, res)==false)
-// {
-// printf("reset_IK_guess service failed.\nExiting..\n");
-// exit(0);
-// }
-
-
- PR2_kinematics pr2_kin;
- JntArray q = JntArray(pr2_kin.nJnts);
-
+ JntArray q = JntArray(right_arm->num_joints_);
q(0) = arm.turretAngle;
q(1) = arm.shoulderLiftAngle;
q(2) = arm.upperarmRollAngle;
@@ -143,14 +154,12 @@
q(6) = arm.wristRollAngle;
Frame f;
- pr2_kin.FK(q,f);
+ right_arm->computeFK(q,f);
Vector start = f.p;
Vector move = r.p-start;
double dist = move.Norm();
move = move/dist;
- std::cout << "Starting trans " << f.p.data[0] << " " << f.p.data[1] << " " << f.p.data[2] << std::endl;
-
RotationalInterpolation_SingleAxis rotInterpolater;
rotInterpolater.SetStartEnd(f.M, r.M);
double total_angle = rotInterpolater.Angle();
@@ -159,19 +168,76 @@
Vector target;
int nSteps = (int)(dist/step_size);
double angle_step = total_angle/nSteps;
- for(int i=0;i<nSteps;i++)
+ bool reachable = true;
+ for(int i=0;i<nSteps && reachable==true;i++)
{
+ printf("[pr2_kinematic_controllers] interpolating...\n");
f.p = start+(i+1)*move*step_size;
f.M = rotInterpolater.Pos(angle_step*(i+1));
- std::cout << "Starting trans " << f.p.data[0] << " " << f.p.data[1] << " " << f.p.data[2] << std::endl;
- publishFrame(isRightArm, f);
+
+ if (isRightArm)
+ reachable=SetRightArmCartesian(f); // services.
+ else
+ publishFrame(isRightArm, f); // old way of doing things. from interpolated_kinematic_controller.
+
usleep(wait_time*1e6);
}
+
f.p = r.p;
f.M = r.M;
- publishFrame(isRightArm, f);
+ if (isRightArm)
+ reachable=SetRightArmCartesian(f); // services.
+ else
+ publishFrame(isRightArm, f); // old way of doing things. from interpolated_kinematic_controller.
+
+ if (reachable==false)
+ printf("[pr2_kinematic_controllers] reachable became FALSE.\n");
}
+ bool SetRightArmCartesian(const Frame &f)
+ {
+ rosgazebo::MoveCartesian::request req;
+ rosgazebo::MoveCartesian::response res;
+ KDL_to_EndEffectorStateMsg(f, req.e);
+
+ if (ros::service::call("set_rightarm_cartesian", req, res)==false)
+ {
+ printf("[pr2_kinematic_controllers] <pr2_kinematic_controllers.cpp> set_rightarm_cartesian service failed.\nExiting..\n");
+ exit(0);
+ }
+
+ return (res.reachable==-1) ? false : true;
+ }
+
+ bool moveAlongGripper(pr2_kinematic_controllers::Float64Int32::request &req, pr2_kinematic_controllers::Float64Int32::response &res)
+ {
+ moveAlongGripper(req.f);
+ return true;
+ }
+
+ void moveAlongGripper(double dist)
+ {
+ JntArray q = JntArray(right_arm->num_joints_);
+
+ q(0) = rightArmPosMsg.turretAngle;
+ q(1) = rightArmPosMsg.shoulderLiftAngle;
+ q(2) = rightArmPosMsg.upperarmRollAngle;
+ q(3) = rightArmPosMsg.elbowAngle;
+ q(4) = rightArmPosMsg.forearmRollAngle;
+ q(5) = rightArmPosMsg.wristPitchAngle;
+ q(6) = rightArmPosMsg.wristRollAngle;
+ Frame f;
+ right_arm->computeFK(q,f);
+ cout<<"current end effector position: "<<f.p<<"\n";
+ cout<<"current end effector rotation: "<<f.M<<"\n";
+ Vector v(0,0,dist);
+ v = f*v;
+ cout<<"final end effector position: "<<v<<"\n";
+ cout<<"final end effector rotation: "<<f.M<<"\n";
+ f.p=v;
+ RunControlLoop(true, f, rightArmPosMsg);
+ }
+
private:
pr2_msgs::EndEffectorState _leftEndEffectorGoal;
@@ -180,6 +246,9 @@
double step_size;
double wait_time;
std_msgs::Float64 float64_msg;
+
+ RobotKinematics pr2_kin;
+ SerialChain *right_arm;
};
Modified: pkg/trunk/manip/pr2_kinematic_controllers/test/test_pr2_kin_con.cc
===================================================================
--- pkg/trunk/manip/pr2_kinematic_controllers/test/test_pr2_kin_con.cc 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/manip/pr2_kinematic_controllers/test/test_pr2_kin_con.cc 2008-08-08 22:35:59 UTC (rev 2837)
@@ -14,7 +14,7 @@
//In shoulder frame x 0.562689
//In shoulder frame y -0.367447
//In shoulder frame z -0.369594
- Rotation r = Rotation::RotZ(DTOR(0));
+ Rotation r = Rotation::RotZ(deg2rad*0);
Vector v(.562689,-.367447,-.369594);
std::cout << " rot: " << std::endl;
Modified: pkg/trunk/manip/teleop_arm_keyboard/Makefile
===================================================================
--- pkg/trunk/manip/teleop_arm_keyboard/Makefile 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/manip/teleop_arm_keyboard/Makefile 2008-08-08 22:35:59 UTC (rev 2837)
@@ -1,16 +1,2 @@
-SRC = teleop_arm_keyboard.cc
-OUT = teleop_arm_keyboard
-PKG = teleop_arm_keyboard
-CXX = g++
-all: $(PKG)
+include $(shell rospack find mk)/cmake.mk
-
-CFLAGS = -g -Wall $(shell rospack export/cpp/cflags $(PKG))
-
-LFLAGS = $(shell rospack export/cpp/lflags $(PKG))
-
-teleop_arm_keyboard: teleop_arm_keyboard.cc
- $(CXX) $(CFLAGS) -o $@ $< $(LFLAGS)
-
-clean:
- rm -rf *.o $(PKG) $(PKG).dSYM
Modified: pkg/trunk/robot_control_loops/pr2_gazebo/manifest.xml
===================================================================
--- pkg/trunk/robot_control_loops/pr2_gazebo/manifest.xml 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/robot_control_loops/pr2_gazebo/manifest.xml 2008-08-08 22:35:59 UTC (rev 2837)
@@ -20,7 +20,8 @@
<depend package="libpr2HW" />
<depend package="math_utils" />
<depend package="string_utils" />
- <depend package="libKDL" />
+ <!-- <depend package="libKDL" /> -->
+ <depend package="robot_kinematics" />
<depend package="robot_mechanism_model" />
<depend package="hw_interface" />
<depend package="gazebo_hardware" />
Modified: pkg/trunk/robot_control_loops/pr2_gazebo/src/pr2_gazebo.cpp
===================================================================
--- pkg/trunk/robot_control_loops/pr2_gazebo/src/pr2_gazebo.cpp 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/robot_control_loops/pr2_gazebo/src/pr2_gazebo.cpp 2008-08-08 22:35:59 UTC (rev 2837)
@@ -87,12 +87,19 @@
/* initialize controllers */
/* */
/***************************************************************************************/
+// controller::ArmController myArm;
+// controller::HeadController myHead;
+// controller::SpineController mySpine;
+// controller::BaseController myBase;
+// controller::LaserScannerController myLaserScanner;
+// controller::GripperController myGripper;
/***************************************************************************************/
/* */
/* initialize ROS Gazebo Nodes */
/* */
/***************************************************************************************/
+// RosGazeboNode rgn(argc,argv,argv[1],myPR2,&myArm,&myHead,&mySpine,&myBase,&myLaserScanner,&myGripper);
RosGazeboNode rgn(argc,argv,argv[1],myPR2);
/***************************************************************************************/
@@ -105,7 +112,7 @@
signal(SIGTERM, (&finalize));
// let rosgazebonode read the xml data from pr2.xml in ros
- rgn.LoadRobotModel();
+// rgn.LoadRobotModel();
// see if we can subscribe models needed
if (rgn.AdvertiseSubscribeMessages() != 0)
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_tablegrasp.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_tablegrasp.world 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_tablegrasp.world 2008-08-08 22:35:59 UTC (rev 2837)
@@ -82,7 +82,7 @@
<!-- The "desk"-->
<model:physical name="desk1_model">
- <xyz> 0.80 -0.21 -0.10</xyz>
+ <xyz> 0.80 -0.21 -0.30</xyz>
<rpy> 0.0 0.0 0.00</rpy>
<body:box name="desk1_legs_body">
<geom:box name="desk1_legs_geom">
@@ -90,7 +90,7 @@
<kd>1.0</kd>
<xyz> 0.0 0.0 0.60</xyz>
<mesh>default</mesh>
- <size>0.5 1.0 0.6</size>
+ <size>0.5 1.0 0.4</size>
<mass> 10.0</mass>
<visual>
<size> 0.5 1.0 0.60</size>
@@ -101,7 +101,7 @@
<geom:box name="desk1_top_geom">
<kp>100000000.0</kp>
<kd>0.1</kd>
- <xyz> 0.0 0.0 0.90</xyz>
+ <xyz> 0.0 0.0 0.88</xyz>
<mesh>default</mesh>
<size>0.75 1.5 0.05</size>
<mass> 10.0</mass>
@@ -150,8 +150,8 @@
<!-- The small box "cup" -->
<model:physical name="object1_model">
- <xyz> 0.835 -0.55 0.95</xyz>
- <rpy> 0.0 0.0 30.0</rpy>
+ <xyz> 0.620 -0.45 0.65</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
<body:box name="object1_body">
<geom:box name="object1_geom">
<kp>100000000.0</kp>
@@ -159,6 +159,8 @@
<mesh>default</mesh>
<size>0.1 0.03 0.03</size>
<mass> 0.05</mass>
+ <mu1> 500.0 </mu1>
+ <mu2> 500.0 </mu2>
<visual>
<size> 0.1 0.030 0.03</size>
<material>Gazebo/PioneerBody</material>
@@ -300,9 +302,8 @@
<!-- base, torso and arms -->
<include embedded="true">
- <xi:include href="pr2.model" />
+ <xi:include href="pr2_xml.model" />
</include>
-
</model:physical>
Added: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml.nolimits
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml.nolimits (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml.nolimits 2008-08-08 22:35:59 UTC (rev 2837)
@@ -0,0 +1,1920 @@
+<?xml version="1.0"?>
+
+<robot name="pr2"><!-- name of the robot-->
+
+ <!-- Begin template definitions for ease of reuse -->
+
+ <const name="robot_inital_position_x" value=" 0.0 " /> <!-- keep 0 for now, because of hardcoded sensor/gripper positions in controllers.cml -->
+ <const name="robot_inital_position_y" value=" 0.0 " /> <!-- keep 0 for now, because of hardcoded sensor/gripper positions in controllers.cml -->
+ <const name="robot_inital_position_z" value=" 0.0*(wheel_radius-base_caster_offset_z/2)/2 " /> <!-- keep 0 for now, because of hardcoded sensor/gripper positions in controllers.cml -->
+ <const name="wheel_clearance_offset" value="0.02" /> <!-- placement robot model origin so wheel are not under ground -->
+
+ <const name="base_size_x" value="0.65" /> <!-- for defining collision geometry -->
+ <const name="base_size_y" value="0.65" /> <!-- for defining collision geometry -->
+ <const name="base_size_z" value="0.26" /> <!-- for defining collision geometry -->
+
+ <const name="wheel_length" value="0.03" /> <!-- FIXME -->
+ <const name="wheel_radius" value="0.079" /> <!-- mp 20080801 -->
+
+ <const name="base_caster_offset_x" value="0.2225" /> <!-- mp 20080801 -->
+ <const name="base_caster_offset_y" value="0.2225" /> <!-- mp 20080801 -->
+ <const name="base_caster_offset_z" value="0.0282" /> <!-- mp 20080801 -->
+
+ <const name="caster_wheel_offset_y" value="0.049" /> <!-- from function spreadsheet -->
+
+ <const name="caster_size_x" value="0.192" /> <!-- for collision geometry -->
+ <const name="caster_size_y" value="0.164" /> <!-- for collision geometry -->
+ <const name="caster_size_z" value="0.013" /> <!-- for collision geometry -->
+ <const name="caster_collision_center_box_center_offset_z" value="0.07" />
+
+ <const name="torso_size_x" value=".432 " /> <!-- for defining collision geometry -->
+ <const name="torso_size_y" value=".620 " /> <!-- for defining collision geometry -->
+ <const name="torso_size_z" value=".823 " /> <!-- for defining collision geometry -->
+
+ <const name="base_torso_offset_x" value="-0.05 " /> <!-- mp 20080801 -->
+ <const name="base_torso_offset_z" value=" 0.739675" /> <!-- mp 20080801 this is the offset for home position: lowest spine setting -->
+
+ <const name="torso_center_box_center_offset_x" value="-.10" /> <!-- FIXME -->
+ <const name="torso_center_box_center_offset_z" value="-.50" /> <!-- FIXME -->
+
+ <const name="torso_max_travel_range" value=" 0.396 " /> <!-- FIXME -->
+
+ <const name="shoulder_pan_size_x" value="0.347" /> <!-- for defining collision geometry -->
+ <const name="shoulder_pan_size_y" value="0.254" /> <!-- for defining collision geometry -->
+ <const name="shoulder_pan_size_z" value="0.646" /> <!-- for defining collision geometry -->
+ <const name="shoulder_pan_center_box_center_offset_x" value=" .05" />
+ <const name="shoulder_pan_center_box_center_offset_z" value="-.20" />
+
+ <const name="shoulder_pan_min_limit" value=" -10*M_PI " /> <!-- FIXME -->
+ <const name="shoulder_pan_max_limit" value=" 10*M_PI " /> <!-- FIXME -->
+
+ <const name="torso_shoulder_pan_offset_y" value="0.188" /> <!-- mp 20080801 -->
+
+ <const name="shoulder_pitch_min_limit" value=" -10*M_PI " />
+ <const name="shoulder_pitch_max_limit" value=" 10*M_PI " />
+
+ <const name="shoulder_pitch_length" value="0.10" /> <!-- for defining collision geometry -->
+ <const name="shoulder_pitch_radius" value="0.12" /> <!-- for defining collision geometry -->
+
+ <const name="shoulder_pan_shoulder_pitch_offset_x" value="0.1" /> <!-- mp 20080801 -->
+
+ <const name="upperarm_roll_size_x" value="0.362" /> <!-- for defining collision geometry -->
+ <const name="upperarm_roll_size_y" value="0.144" /> <!-- for defining collision geometry -->
+ <const name="upperarm_roll_size_z" value="0.157" /> <!-- for defining collision geometry -->
+ <const name="upperarm_roll_center_box_center_offset_x" value=".30" /> <!-- from origin of mesh to center of box-geom -->
+
+
+ <const name="elbow_flex_min_limit" value=" -10*M_PI " />
+ <const name="elbow_flex_max_limit" value=" 10*M_PI " />
+
+ <const name="elbow_flex_length" value="0.08" /> <!-- for defining collision geometry -->
+ <const name="elbow_flex_radius" value="0.1" /> <!-- for defining collision geometry -->
+
+ <const name="upperarm_roll_elbow_flex_offset_x" value="0.4" /> <!-- mp 20080801 -->
+
+ <const name="forearm_roll_size_x" value="0.27" /> <!-- for defining collision geometry -->
+ <const name="forearm_roll_size_y" value="0.12" /> <!-- for defining collision geometry -->
+ <const name="forearm_roll_size_z" value="0.08" /> <!-- for defining collision geometry -->
+ <const name="forearm_roll_center_box_center_offset_x" value=" 0.22 " /> <!-- FIXME -->
+
+ <const name="elbow_roll_wrist_flex_offset_x" value="0.32025" /> <!-- mp 20080801 -->
+
+ <const name="wrist_flex_radius" value="0.033 " /> <!-- for defining collision geometry -->
+ <const name="wrist_flex_length" value="0.103 " /> <!-- for defining collision geometry -->
+
+ <const name="wrist_flex_min_limit" value=" -10*M_PI " />
+ <const name="wrist_flex_max_limit" value=" 10*M_PI " />
+
+ <const name="gripper_roll_size_x" value="0.11" /> <!-- for defining collision geometry -->
+ <const name="gripper_roll_size_y" value="0.10" /> <!-- for defining collision geometry -->
+ <const name="gripper_roll_size_z" value="0.05" /> <!-- for defining collision geometry -->
+ <const name="gripper_roll_center_box_center_offset_x" value="0.05" /> <!-- FIXME -->
+
+
+
+ <const name="head_pan_min_limit" value="-2.9322" /> <!-- FIXME -->
+ <const name="head_pan_max_limit" value="2.9322" /> <!-- FIXME -->
+
+ <const name="torso_head_pan_offset_x" value="0.0" /> <!-- mp 20080801 -->
+ <const name="torso_head_pan_offset_z" value="0.3815" /> <!-- mp 20080801 -->
+
+ <const name="head_pan_size_x" value="0.188" /> <!-- for defining collision geometry -->
+ <const name="head_pan_size_y" value="0.219" /> <!-- for defining collision geometry -->
+ <const name="head_pan_size_z" value="0.137" /> <!-- for defining collision geometry -->
+
+
+ <const name="head_tilt_min_limit" value=" -M_PI/6 " /> <!-- FIXME -->
+ <const name="head_tilt_max_limit" value=" M_PI/3 " /> <!-- FIXME -->
+
+ <const name="head_pan_head_tilt_offset_x" value="0.058" /> <!-- mp 20080801 -->
+
+ <const name="head_tilt_size_x" value="0.164" /> <!-- for defining collision geometry -->
+ <const name="head_tilt_size_y" value="0.253" /> <!-- for defining collision geometry -->
+ <const name="head_tilt_size_z" value="0.181" /> <!-- for defining collision geometry -->
+
+ <!-- stereo camera -->
+ <const name="head_tilt_stereo_offset_x" value="0.0232" /> <!-- mp 20080801 -->
+ <const name="head_tilt_stereo_offset_z" value="0.0645" /> <!-- mp 20080801 -->
+ <const name="stereo_size_x" value=" 0.05 " /> <!-- for definint visual and collision geometry -->
+ <const name="stereo_size_y" value=" 0.10 " /> <!-- for definint visual and collision geometry -->
+ <const name="stereo_size_z" value=" 0.05 " /> <!-- for definint visual and collision geometry -->
+ <const name="stereo_center_box_center_offset_x" value=" 0.00 " /> <!-- from center of stereo to stereo mount point, which is provided above -->
+ <const name="stereo_center_box_center_offset_z" value=" 0.05 " /> <!-- from center of stereo to stereo mount point, which is provided above -->
+
+ <!-- ptz cameras -->
+ <const name="torso_ptz_pan_left_offset_x" value=" 0.0000" /> <!-- mp 20080801 -->
+ <const name="torso_ptz_pan_left_offset_y" value=" 0.1975" /> <!-- mp 20080801 -->
+ <const name="torso_ptz_pan_left_offset_z" value=" 0.2265" /> <!-- mp 20080801 -->
+ <const name="torso_ptz_pan_right_offset_x" value=" 0.0000" /> <!-- mp 20080801 -->
+ <const name="torso_ptz_pan_right_offset_y" value="-0.1975" /> <!-- mp 20080801 -->
+ <const name="torso_ptz_pan_right_offset_z" value=" 0.2265" /> <!-- mp 20080801 -->
+
+ <const name="ptz_pan_ptz_tilt_left_offset_x" value=" 0.0000" /> <!-- FIXME -->
+ <const name="ptz_pan_ptz_tilt_left_offset_y" value=" 0.0300" /> <!-- FIXME -->
+ <const name="ptz_pan_ptz_tilt_left_offset_z" value=" 0.0000" /> <!-- FIXME -->
+ <const name="ptz_pan_ptz_tilt_right_offset_x" value=" 0.0000" /> <!-- FIXME -->
+ <const name="ptz_pan_ptz_tilt_right_offset_y" value="-0.0300" /> <!-- FIXME -->
+ <const name="ptz_pan_ptz_tilt_right_offset_z" value=" 0.0000" /> <!-- FIXME -->
+
+ <const name="ptz_pan_min_limit" value=" -M_PI/2 " /> <!-- FIXME -->
+ <const name="ptz_pan_max_limit" value=" M_PI/2 " /> <!-- FIXME -->
+ <const name="ptz_tilt_min_limit" value=" -M_PI/2 " /> <!-- FIXME -->
+ <const name="ptz_tilt_max_limit" value=" M_PI/2 " /> <!-- FIXME -->
+
+ <const name="ptz_pan_radius" value=" 0.05 " /> <!-- for definint visual and collision geometry -->
+ <const name="ptz_pan_length" value=" 0.05 " /> <!-- for definint visual and collision geometry -->
+ <const name="ptz_pan_size_x" value=" 0.05 " /> <!-- for definint visual and collision geometry -->
+ <const name="ptz_pan_size_y" value=" 0.05 " /> <!-- for definint visual and collision geometry -->
+ <const name="ptz_pan_size_z" value=" 0.05 " /> <!-- for definint visual and collision geometry -->
+ <const name="ptz_pan_left_center_box_center_offset_x" value=" 0.00 " /> <!-- for definint visual and collision geometry -->
+ <const name="ptz_pan_left_center_box_center_offset_y" value=" 0.025 " /> <!-- for definint visual and collision geometry -->
+ <const name="ptz_pan_left_center_box_center_offset_z" value=" 0.00 " /> <!-- for definint visual and collision geometry -->
+ <const name="ptz_pan_right_center_box_center_offset_x" value=" 0.00 " /> <!-- for definint visual and collision geometry -->
+ <const name="ptz_pan_right_center_box_center_offset_y" value="-0.025 " /> <!-- for definint visual and collision geometry -->
+ <const name="ptz_pan_right_center_box_center_offset_z" value=" 0.00 " /> <!-- for definint visual and collision geometry -->
+
+ <const name="ptz_tilt_radius" value=" 0.03 " /> <!-- for definint visual and collision geometry -->
+ <const name="ptz_tilt_length" value=" 0.03 " /> <!-- for definint visual and collision geometry -->
+ <const name="ptz_tilt_size_x" value=" 0.03 " /> <!-- for definint visual and collision geometry -->
+ <const name="ptz_tilt_size_y" value=" 0.03 " /> <!-- for definint visual and collision geometry -->
+ <const name="ptz_tilt_size_z" value=" 0.03 " /> <!-- for definint visual and collision geometry -->
+
+ <!-- =========================== l finger proximal digit =========================== -->
+ <const name="gripper_roll_finger_l_offset_x" value=" 0.07751" /> <!-- mp 20080801 -->
+ <const name="gripper_roll_finger_l_offset_y" value=" 0.0100 " /> <!-- mp 20080801 -->
+ <const name="gripper_roll_finger_l_offset_z" value=" 0.0000 " /> <!-- mp 20080801 -->
+
+ <const name="finger_l_size_x" value="0.131" /> <!-- for defining collision geometry -->
+ <const name="finger_l_size_y" value="0.056" /> <!-- for defining collision geometry -->
+ <const name="finger_l_size_z" value="0.049" /> <!-- for defining collision geometry -->
+ <const name="finger_l_center_box_center_offset_x" value="0.00" /> <!-- FIXME -->
+ <const name="finger_l_center_box_center_offset_y" value="0.00" /> <!-- FIXME -->
+
+
+ <!-- =========================== l finger tip (distal digit) =========================== -->
+ <const name="finger_l_finger_tip_l_offset_x" value=" 0.0915 " /> <!-- mp 20080801 -->
+ <const name="finger_l_finger_tip_l_offset_y" value=" 0.0000 " /> <!-- mp 20080801 -->
+ <const name="finger_l_finger_tip_l_offset_z" value=" 0.0000 " /> <!-- mp 20080801 -->
+
+ <const name="finger_tip_l_size_x" value="0.053" /> <!-- for defining collision geometry -->
+ <const name="finger_tip_l_size_y" value="0.040" /> <!-- for defining collision geometry -->
+ <const name="finger_tip_l_size_z" value="0.023" /> <!-- for defining collision geometry -->
+ <const name="finger_tip_l_center_box_center_offset_x" value="0.00" /> <!-- FIXME -->
+ <const name="finger_tip_l_center_box_center_offset_y" value="0.00" /> <!-- FIXME -->
+
+ <!-- =========================== r finger proximal digit =========================== -->
+ <const name="gripper_roll_finger_r_offset_x" value=" 0.07751" /> <!-- mp 20080801 -->
+ <const name="gripper_roll_finger_r_offset_y" value="-0.0100 " /> <!-- mp 20080801 -->
+ <const name="gripper_roll_finger_r_offset_z" value=" 0.0000 " /> <!-- mp 20080801 -->
+
+ <const name="finger_r_size_x" value="0.131" /> <!-- for defining collision geometry -->
+ <const name="finger_r_size_y" value="0.056" /> <!-- for defining collision geometry -->
+ <const name="finger_r_size_z" value="0.049" /> <!-- for defining collision geometry -->
+ <const name="finger_r_center_box_center_offset_x" value="0.00" /> <!-- FIXME -->
+ <const name="finger_r_center_box_center_offset_y" value="0.00" /> <!-- FIXME -->
+
+ <!-- =========================== r finger tip (distal digit) =========================== -->
+ <const name="finger_r_finger_tip_r_offset_x" value=" 0.0915 " /> <!-- mp 20080801 -->
+ <const name="finger_r_finger_tip_r_offset_y" value=" 0.0000 " /> <!-- mp 20080801 -->
+ <const name="finger_r_finger_tip_r_offset_z" value=" 0.0000 " /> <!-- mp 20080801 -->
+
+ <const name="finger_tip_r_size_x" value="0.053" /> <!-- for defining collision geometry -->
+ <const name="finger_tip_r_size_y" value="0.040" /> <!-- for defining collision geometry -->
+ <const name="finger_tip_r_size_z" value="0.023" /> <!-- for defining collision geometry -->
+ <const name="finger_tip_r_center_box_center_offset_x" value="0.00" /> <!-- FIXME -->
+ <const name="finger_tip_r_center_box_center_offset_y" value="0.00" /> <!-- FIXME -->
+
+
+
+ <!-- to be used by sensors -->
+ <const name="torso_tilt_laser_offset_x" value="0.10000" /> <!-- mp 20080801 -->
+ <const name="torso_tilt_laser_offset_z" value="0.19525" /> <!-- mp 20080801 -->
+ <const name="tilt_laser_center_box_center_offset_z" value="0.0" /> <!-- FIXME -->
+
+ <const name="base_base_laser_offset_x" value="0.275" /> <!-- mp 20080801 -->
+ <const name="base_base_laser_offset_z" value="0.182" /> <!-- mp 20080801 -->
+ <const name="base_laser_center_box_center_offset_z" value="0.12" /> <!-- FIXME -->
+
+ <const name="gripper_roll_camera_offset_x" value="0.05" /> <!-- this is a guess, please change me -->
+ <const name="gripper_roll_camera_offset_y" value="0 " /> <!-- this is a guess, please change me -->
+ <const name="gripper_roll_camera_offset_z" value="0.03" /> <!-- this is a guess, please change me -->
+
+ <const name="forearm_roll_camera_offset_x" value="0.10" /> <!-- this is a guess, please change me -->
+ <const name="forearm_roll_camera_offset_y" value="0 " /> <!-- this is a guess, please change me -->
+ <const name="forearm_roll_camera_offset_z" value="0.05" /> <!-- this is a guess, please change me -->
+
+ <!-- End constant dimensions -->
+
+ <templates>
+ <define template="pr2_caster_visual">
+ <xyz>0 0 0</xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
+ <rpy>0 0 0 </rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <material>Gazebo/Green</material>
+ <scale>1 1 1</scale>
+ <geometry type="mesh" name="pr2_caster_mesh_file">
+ <filename>caster</filename><!-- mesh specified using an obj file -->
+ </geometry>
+ </define>
+ <define template="pr2_caster_collision">
+ <xyz>0 0 caster_collision_center_box_center_offset_z</xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
+ <rpy>0.0 0.0 0.0 </rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <geometry type="box"><!-- geometry specified using a simple geometric object -->
+ <size>caster_size_x caster_size_y caster_size_z</size>
+ </geometry>
+ </define>
+ <define template="pr2_caster_inertial">
+ <mass> 3.473082 </mass>
+ <com> 0 0 0 </com> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
+ <inertia> 0.012411765597 -0.000711733678 0.00050272983 0.015218160428 -0.000004273467 0.011763977943 </inertia>
+ </define>
+
+
+ <define template="pr2_caster_joint" type="revolute">
+ <axis> 0 0 1 </axis> <!-- direction of the joint in the local coordinate frame -->
+ <anchor>0 0 0</anchor> <!-- point on the joint relative to the local coordinate frame -->
+ <calibration>1.5 -1 </calibration> <!-- Calibration stop/flag indicating location and then direction -->
+ </define>
+
+ <define template="pr2_wheel_left_visual">
+ <xyz>0 0 0</xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
+ <rpy>0.0 0.0 0.0 </rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <material>Gazebo/Blue</material>
+ <scale>1 1 1</scale>
+ <geometry type="mesh" name="pr2_wheel_mesh_file"><!-- geometry specified using a simple geometric object -->
+ <filename>wheel_left</filename>
+ </geometry>
+ </define>
+
+ <define template="pr2_wheel_right_visual">
+ <xyz>0 0 0</xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
+ <rpy>0.0 0.0 0.0 </rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <material>Gazebo/Red</material>
+ <scale>1 1 1</scale>
+ <geometry type="mesh" name="pr2_wheel_mesh_file"><!-- geometry specified using a simple geometric object -->
+ <filename>wheel_right</filename>
+ </geometry>
+ </define>
+
+ <define template="pr2_wheel_collision">
+ <xyz>0 0 0</xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
+ <rpy>M_PI/2 0.0 0.0 </rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <geometry type="cylinder"><!-- geometry specified using a simple geometric object -->
+ <size>wheel_radius wheel_length</size>
+ </geometry>
+
+ <data name="friction_coefficients" type="gazebo">
+ <!-- <elem key="mu1" value="5.0" /> --> <!-- john needed so wheel can slip and steer -->
+ <!-- <elem key="mu2" value="5.0" /> --> <!-- john needed so wheel can slip and steer -->
+ </data>
+
+ </define>
+
+ <define template="pr2_wheel_inertial">
+ <mass> 0.44036 </mass>
+ <com> 0 0 0 </com> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
+ <inertia> 0.012411765597 -0.000711733678 0.00050272983 0.015218160428 -0.000004273467 0.011763977943 </inertia>
+ </define>
+
+ <define template="pr2_wheel_joint" type="revolute">
+ <axis> 0 1 0 </axis> <!-- direction of the joint in the local coordinate frame -->
+ <anchor>0 0 0</anchor> <!-- point on the joint relative to the local coordinate frame -->
+ <calibration>1.5 -1 </calibration> <!-- Calibration stop/flag indicating location and then direction -->
+ </define>
+
+ <define template="pr2_shoulder_pan_joint" type="revolute">
+ <axis> 0 0 1 </axis> <!-- direction of the joint in a global coordinate frame -->
+ <anchor>0 0 0 </anchor> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
+ <limit> shoulder_pan_min_limit shoulder_pan_max_limit </limit>
+ <calibration>1.5 -1 </calibration> <!-- Calibration stop/flag indicating location and then direction -->
+ </define>
+
+ <define template="pr2_shoulder_pan_inertial">
+ <mass> 16.29553 </mass>
+ <com> -0.005215 -0.030552 0.0 </com> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
+ <inertia>0.793291393007 0.003412032973 0.0096614481 0.818419457224 -0.033999499551 0.13915007406 </inertia>
+ </define>
+
+ <define template="pr2_shoulder_pan_collision">
+ <xyz>shoulder_pan_center_box_center_offset_x 0 shoulder_pan_center_box_center_offset_z</xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
+ <rpy>0 0 0 </rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <geometry type="box">
+ <size> shoulder_pan_size_x shoulder_pan_size_y shoulder_pan_size_z</size>
+ </geometry>
+ </define>
+
+ <define template="pr2_shoulder_pan_visual">
+ <xyz>0 0 0 </xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
+ <rpy>0 0 0</rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <material>Gazebo/Blue</material>
+ <scale>1 1 1</scale>
+ <geometry type="mesh" name="pr2_sholder_pan_mesh_file">
+ <filename>sh-pan</filename>
+ </geometry>
+ </define>
+
+ <define template="pr2_shoulder_pitch_joint" type="revolute">
+ <axis> 0 1 0 </axis> <!-- direction of the joint in a global coordinate frame -->
+ <anchor>0 0 0 </anchor> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
+ <limit> shoulder_pitch_min_limit shoulder_pitch_max_limit </limit>
+ <calibration>1.5 -1 </calibration> <!-- Calibration stop/flag indicating location and then direction -->
+ </define>
+
+ <define template="pr2_shoulder_pitch_inertial">
+ <mass> 2.41223 </mass>
+ <com> -0.035895 0.014422 -0.0028263</com> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
+ <inertia> 0.016640333585 0.002696462583 0.001337742275 0.017232603914 -0.003605106514 0.018723553425</inertia>
+ </define>
+
+ <define template="pr2_shoulder_pitch_collision">
+ <xyz>shoulder_pitch_length/2 0 0 </xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
+ <rpy>M_PI/2 0.0 0.0 </rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <material>Gazebo/Grey</material>
+ <geometry type="cylinder">
+ <size> shoulder_pitch_radius shoulder_pitch_length </size>
+ </geometry>
+ </define>
+
+ <define template="pr2_shoulder_pitch_visual">
+ <xyz>0 0 0 </xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
+ <rpy>0 0 0 </rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <material>Gazebo/Grey</material>
+ <scale>1 1 1</scale>
+ <geometry type="mesh" name="pr2_sholder_pitch_mesh_file">
+ <filename>sh-pitch</filename>
+ </geometry>
+ </define>
+
+ <define template="pr2_upperarm_roll_joint" type="revolute">
+ <axis> 1 0 0 </axis> <!-- direction of the joint in a global coordinate frame -->
+ <anchor>0 0 0 </anchor> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
+ <limit> -10*M_PI 10*M_PI </limit>
+ <calibration>1.5 -1 </calibration> <!-- Calibration stop/flag indicating location and then direction -->
+ </define>
+
+ <define template="pr2_upperarm_roll_inertial">
+ <mass> 4.9481 </mass>
+ <com> 0.21227 0.001205 -0.016293 </com> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame --> <!-- FIXME John switched x and z for now -->
+ <inertia> 0.073060715309 0.000547745916 0.000119476885 0.072124510748 0.001544932307 0.013383284908</inertia>
+ </define>
+
+ <define template="pr2_upperarm_roll_collision">
+ <xyz>upperarm_roll_center_box_center_offset_x 0 0</xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
+ <rpy>0.0 0.0 0.0 </rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <geometry type="box">
+ <size> upperarm_roll_size_x upperarm_roll_size_y upperarm_roll_size_z </size>
+ </geometry>
+ </define>
+
+ <define template="pr2_upperarm_roll_visual">
+ <xyz>0 0 0</xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
+ <rpy>0 0 0 </rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <material>Gazebo/Green</material>
+ <scale>1 1 1</scale>
+ <geometry type="mesh" name="pr2_sholder_roll_mesh_file">
+ <filename>sh-roll</filename>
+ </geometry>
+ </define>
+
+ <define template="pr2_elbow_flex_joint" type="revolute">
+ <axis> 0 1 0 </axis> <!-- direction of the joint in a global coordinate frame -->
+ <anchor>0 0 0 </anchor> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
+ <limit> elbow_flex_min_limit elbow_flex_max_limit </limit>
+ <calibration>1.5 -1 </calibration> <!-- Calibration stop/flag indicating location and then direction -->
+ </define>
+
+ <define template="pr2_elbow_flex_inertial">
+ <mass> 1.689246 </mass>
+ <com> -0.011638 0.013173 0.001228 </com> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
+ <inertia> 0.003275298548 0.000292046674 -0.000077359282 0.003236815206 -0.000001162155 0.00410053774 </inertia>
+ </define>
+
+ <define template="pr2_elbow_flex_collision">
+ <xyz>0 0 0 </xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
+ <rpy>M_PI/2 0.0 0.0 </rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <material>Gazebo/Grey</material>
+ <geometry type="cylinder">
+ <size>elbow_flex_radius elbow_flex_length </size>
+ ...
[truncated message content] |
