Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[3125] pkg/trunk
|
From: <adv...@us...> - 2008-08-15 18:39:20
|
Revision: 3125
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3125&view=rev
Author: advaitjain
Date: 2008-08-15 18:39:28 +0000 (Fri, 15 Aug 2008)
Log Message:
-----------
documentation and licenses.
Modified Paths:
--------------
pkg/trunk/deprecated/libKDL/src/kdl_kinematics.cpp
pkg/trunk/deprecated/libKDL/test/dynamics.cpp
pkg/trunk/deprecated/libKDL/test/pr2_kin_test.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/teleop_arm_keyboard.cc
Removed Paths:
-------------
pkg/trunk/deprecated/libKDL/doc/
Modified: pkg/trunk/deprecated/libKDL/src/kdl_kinematics.cpp
===================================================================
--- pkg/trunk/deprecated/libKDL/src/kdl_kinematics.cpp 2008-08-15 18:14:39 UTC (rev 3124)
+++ pkg/trunk/deprecated/libKDL/src/kdl_kinematics.cpp 2008-08-15 18:39:28 UTC (rev 3125)
@@ -1,4 +1,47 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+// Author: Advait Jain
+
+/**
+
+ @mainpage
+
+ @htmlinclude manifest.html
+
+ @b libKDL has been @b deprecated by @b robot_kinematics. It served as a wrapper around KDL to make it easier to use FK and IK.
+
+ **/
+
+
+
+
#include "libKDL/kdl_kinematics.h"
using namespace KDL;
Modified: pkg/trunk/deprecated/libKDL/test/dynamics.cpp
===================================================================
--- pkg/trunk/deprecated/libKDL/test/dynamics.cpp 2008-08-15 18:14:39 UTC (rev 3124)
+++ pkg/trunk/deprecated/libKDL/test/dynamics.cpp 2008-08-15 18:39:28 UTC (rev 3125)
@@ -1,5 +1,35 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+// Author: Advait Jain
+
#include "libKDL/kdl_kinematics.h"
#include "libKDL/dynamics.h"
Modified: pkg/trunk/deprecated/libKDL/test/pr2_kin_test.cpp
===================================================================
--- pkg/trunk/deprecated/libKDL/test/pr2_kin_test.cpp 2008-08-15 18:14:39 UTC (rev 3124)
+++ pkg/trunk/deprecated/libKDL/test/pr2_kin_test.cpp 2008-08-15 18:39:28 UTC (rev 3125)
@@ -1,4 +1,35 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+// Author: Advait Jain
+
+
#include "libKDL/kdl_kinematics.h"
#include <sys/time.h>
Modified: pkg/trunk/manip/overhead_grasp_behavior/manifest.xml
===================================================================
--- pkg/trunk/manip/overhead_grasp_behavior/manifest.xml 2008-08-15 18:14:39 UTC (rev 3124)
+++ pkg/trunk/manip/overhead_grasp_behavior/manifest.xml 2008-08-15 18:39:28 UTC (rev 3125)
@@ -3,7 +3,6 @@
</description>
<author>Advait Jain</author>
<license>BSD</license>
-<url>http://pr.willowgarage.com</url>
<depend package="roscpp"/>
<depend package="std_msgs"/>
<depend package="pr2Core"/>
Modified: pkg/trunk/manip/overhead_grasp_behavior/src/overhead_grasp.cc
===================================================================
--- pkg/trunk/manip/overhead_grasp_behavior/src/overhead_grasp.cc 2008-08-15 18:14:39 UTC (rev 3124)
+++ pkg/trunk/manip/overhead_grasp_behavior/src/overhead_grasp.cc 2008-08-15 18:39:28 UTC (rev 3125)
@@ -1,3 +1,86 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+/**
+
+ @mainpage
+
+ @htmlinclude manifest.html
+
+ @b overhead_grasp_behavior is the state machine that can be used to step the PR2 through the different states such as position_camera_above_object, segment_object, grasp_from_above etc. This is the object grasping behavior that we use at the Healthcare Robotics Lab at Georgia Tech. More documentation at the wiki page on Behavior Based Grasping.
+
+ <hr>
+
+ @section usage Usage
+ @verbatim
+ The correct pr2 model files for gazebo need to be generated. Follow step-by-step instructions on the wiki.
+ @endverbatim
+
+ <hr>
+
+ @section topic ROS topics
+
+ Subscribes to (name/type):
+ - @b "object_position" / @b std_msgs::Point3DFloat32 : 3d Pose of the object. This is the cheat laser pointer interface.
+ - @b "rightarm_tooltip_cartesian" / @b pr2_msgs::EndEffectorState : current cartesian pose of the end effector.
+
+ Publishes to (name/type):
+ - @b "right_pr2arm_set_end_effector" / @b pr2_msgs::EndEffectorState : desired end effector pose for the right arm. Should be a service.
+ - @b "interpolate_step_size" / @b std_msgs::Float64 : step size for interpolation. (pr2_kinematic_controllers)
+ - @b "interpolate_wait_time" / @b std_msgs::Float64 : wait time for interpolation. (pr2_kinematic_controllers)
+
+ <hr>
+
+ @section parameters ROS parameters
+
+ - None
+
+ <hr>
+
+ @section services ROS services
+
+ Advertises (name/type):
+
+ - None
+
+ Calls (name/type):
+
+ - @b "hrl_grasp" / @b gmmseg::hrl_grasp : tell node gmmseg to segment the image, get back x,y,theta of the object in camera coordiante frame.
+ - @b "operate_right_gripper" / @b rosgazebo::GripperCmd : set gap and force for the gripper.
+ - @b "move_along_gripper" / @b pr2_kinematic_controllers::Float64Int32 : reaching motion.
+
+
+ **/
+
+// Author: Advait Jain
+
#include <termios.h>
#include <signal.h>
#include <math.h>
@@ -23,181 +106,154 @@
class OverheadGrasper : public ros::node
{
- public:
- // 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;
+ public:
+ // 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.16;
- Vector CAMERA_ENDEFFECTOR; // position of camera relative to end effector in end effector frame.
+ Frame right_tooltip_frame;
+ Vector objectPosition;
+ const static double PR2_GRIPPER_LENGTH = 0.16;
+ 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"); // 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);
+ public:
+ OverheadGrasper(void) : ros::node("overhead_grasper")
+ {
+ 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.020-0.7987,-0.15,0.8269);
- gazebo_to_arm_vector = Vector(0.81-0.82025,-0.20,0.739675);
+// 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.
- }
+ CAMERA_ENDEFFECTOR = Vector(0.0, 0.,0.05); // position of camera relative to end effector in base frame.
+ }
- void KDL_to_EndEffectorStateMsg(const Frame& f, pr2_msgs::EndEffectorState &efs)
- {
- efs.set_rot_size(9);
- efs.set_trans_size(3);
- 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 KDL_to_EndEffectorStateMsg(const Frame& f, pr2_msgs::EndEffectorState &efs)
+ {
+ efs.set_rot_size(9);
+ efs.set_trans_size(3);
+ 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 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);
+ 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);
- }
+ 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.;
+ // Vector v is shoulder coordinate frame.
+// void positionEyecamOverObject(Vector v)
+ void positionEyecamOverObject()
+ {
+ 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);
+ }
- rightarm.turretAngle = deg2rad*-20;
- rightarm.shoulderLiftAngle = deg2rad*-20;
- rightarm.upperarmRollAngle = deg2rad*180;
- rightarm.elbowAngle = deg2rad*-30;
- rightarm.forearmRollAngle = 0;
- rightarm.wristPitchAngle = 0;
- rightarm.wristRollAngle = 0;
- rightarm.gripperForceCmd = 50;
- rightarm.gripperGapCmd = 0.;
+ 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);
+ cout<<"move: "<<move<<"\n";
- publish("cmd_rightarmconfig",rightarm);
- }
+ move += Rotation::RotY(deg2rad*90)*CAMERA_ENDEFFECTOR;
- // Vector v is shoulder coordinate frame.
-// void positionEyecamOverObject(Vector v)
- void positionEyecamOverObject()
- {
- 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);
- }
+ if (res.theta>0)
+ res.theta = deg2rad*90-res.theta;
+ else
+ res.theta = -1*(res.theta+deg2rad*90);
- 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);
- cout<<"move: "<<move<<"\n";
+ Rotation r = Rotation::RotY(deg2rad*90)*Rotation::RotY(deg2rad*90)*Rotation::RotZ(res.theta); // 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);
+ }
- move += Rotation::RotY(deg2rad*90)*CAMERA_ENDEFFECTOR;
+ void objectPosition_cb(void)
+ {
+// cout<<"object's z coord: "<<objectPosMsg.z<<"\n";
+ objectPosition = Vector(objectPosMsg.x,objectPosMsg.y,objectPosMsg.z) - gazebo_to_arm_vector;
+ }
- if (res.theta>0)
- res.theta = deg2rad*90-res.theta;
- else
- res.theta = -1*(res.theta+deg2rad*90);
+ void currentRightArmPosCartesian_cb(void)
+ {
+ EndEffectorStateMsg_to_KDL(this->rightEndEffectorMsg, this->right_tooltip_frame);
+ }
- Rotation r = Rotation::RotY(deg2rad*90)*Rotation::RotY(deg2rad*90)*Rotation::RotZ(res.theta); // 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 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 objectPosition_cb(void)
- {
-// cout<<"object's z coord: "<<objectPosMsg.z<<"\n";
- objectPosition = Vector(objectPosMsg.x,objectPosMsg.y,objectPosMsg.z) - gazebo_to_arm_vector;
- }
+ 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 currentRightArmPosCartesian_cb(void)
- {
- EndEffectorStateMsg_to_KDL(this->rightEndEffectorMsg, this->right_tooltip_frame);
- }
+ 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 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 printTooltipTransformation(void)
+ {
+ cout<<"End effector transformation: "<<this->right_tooltip_frame<<"\n";
+ }
- 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);
- }
+ 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);
+ }
};
@@ -207,29 +263,29 @@
int main(int argc, char **argv)
{
- ros::init(argc, argv);
- OverheadGrasper ohGrasper;
+ ros::init(argc, argv);
+ OverheadGrasper ohGrasper;
- std_msgs::Float64 float64_msg;
- sleep(1);
+ std_msgs::Float64 float64_msg;
+ sleep(1);
- double interpolate_step_size = 0.01;
- float64_msg.data = interpolate_step_size;
- ohGrasper.publish("interpolate_step_size", float64_msg);
-
- double interpolate_wait_time = .3;
- float64_msg.data = interpolate_wait_time;
- ohGrasper.publish("interpolate_wait_time", float64_msg);
+ double interpolate_step_size = 0.01;
+ float64_msg.data = interpolate_step_size;
+ ohGrasper.publish("interpolate_step_size", float64_msg);
+
+ double interpolate_wait_time = .3;
+ float64_msg.data = interpolate_wait_time;
+ ohGrasper.publish("interpolate_wait_time", float64_msg);
//---- now for the keyboard driven state machine ------
- while(1)
- {
+ while(1)
+ {
signal(SIGINT,quit);
- keyboardLoop(ohGrasper);
- }
+ keyboardLoop(ohGrasper);
+ }
- return 0;
+ return 0;
}
@@ -240,85 +296,82 @@
void quit(int sig)
{
- ros::fini();
- tcsetattr(kfd, TCSANOW, &cooked);
- exit(0);
+ ros::fini();
+ tcsetattr(kfd, TCSANOW, &cooked);
+ exit(0);
}
void keyboardLoop(OverheadGrasper &n)
{
- char c;
+ char c;
- // get the console in raw mode
- tcgetattr(kfd, &cooked);
- memcpy(&raw, &cooked, sizeof(struct termios));
- raw.c_lflag &=~ (ICANON | ECHO);
- raw.c_cc[VEOL] = 1;
- raw.c_cc[VEOF] = 2;
- tcsetattr(kfd, TCSANOW, &raw);
+ // get the console in raw mode
+ tcgetattr(kfd, &cooked);
+ memcpy(&raw, &cooked, sizeof(struct termios));
+ raw.c_lflag &=~ (ICANON | ECHO);
+ raw.c_cc[VEOL] = 1;
+ raw.c_cc[VEOF] = 2;
+ tcsetattr(kfd, TCSANOW, &raw);
- puts("Reading from keyboard");
- puts("---------------------------");
- printf("Numbers starting from 1 to step through the states\n");
- puts("---------------------------");
+ puts("Reading from keyboard");
+ puts("---------------------------");
+ printf("Numbers starting from 1 to step through the states\n");
+ puts("---------------------------");
- for(;;)
- {
- // get the next event from the keyboard
- if(read(kfd, &c, 1) < 0)
- {
- perror("read():");
- exit(-1);
- }
+ for(;;)
+ {
+ // get the next event from the keyboard
+ if(read(kfd, &c, 1) < 0)
+ {
+ perror("read():");
+ exit(-1);
+ }
- switch(c)
- {
- case '1':
- n.overheadGraspPosture();
- break;
- case '2':
- {
- n.positionEyecamOverObject();
- }
- break;
- case '3':
- n.graspFromAbove();
- break;
- case '4':
- n.pickUpObject();
- break;
+ switch(c)
+ {
+ case '2':
+ {
+ n.positionEyecamOverObject();
+ }
+ break;
+ case '3':
+ n.graspFromAbove();
+ break;
+ case '4':
+ n.pickUpObject();
+ break;
- case 't':
- n.moveArmSegmentation();
- break;
+ case 't':
+ n.moveArmSegmentation();
+ break;
- case 'e':
- n.printTooltipTransformation();
- 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);
- }
+ 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;
- }
- }
+ // 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-15 18:14:39 UTC (rev 3124)
+++ pkg/trunk/manip/pr2_kinematic_controllers/manifest.xml 2008-08-15 18:39:28 UTC (rev 3125)
@@ -1,15 +1,13 @@
<package>
- <description brief='Higher level controllers for kinematics. Sourced from interpolated_kinematic_controller'>
+ <description> Higher level controllers for kinematics. Sourced from interpolated_kinematic_controller.
</description>
<author>Advait Jain</author>
<license>BSD</license>
- <url>http://pr.willowgarage.com</url>
<depend package="roscpp"/>
<depend package="std_msgs"/>
<depend package="pr2_msgs"/>
<depend package="pr2Core"/>
<depend package="libpr2API"/>
- <!-- <depend package="libKDL"/> -->
<depend package="robot_kinematics"/>
<depend package="pr2_gazebo"/>
<export>
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-15 18:14:39 UTC (rev 3124)
+++ pkg/trunk/manip/pr2_kinematic_controllers/src/pr2_kinematic_controllers.cc 2008-08-15 18:39:28 UTC (rev 3125)
@@ -1,3 +1,86 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+// Author: Advait Jain
+
+/**
+
+ @mainpage
+
+ @htmlinclude manifest.html
+
+ @b pr2_kinematic_controllers provides functionality for moving the end effector to a desired cartesian pose (with interpolation), moving the end effector along the direction of the gripper (a reaching motion).
+
+ <hr>
+
+ @section usage Usage
+ @verbatim
+ $ pr2_kinematic_controllers [standard ROS args]
+ @endverbatim
+
+ <hr>
+
+ @section topic ROS topics
+
+ Subscribes to (name/type):
+ - @b "right_pr2arm_set_end_effector" / @b pr2_msgs::EndEffectorState : Desired pose of the right arm's end effector. (This should probably be a service advertised by pr2_kinematic_controllers).
+ - @b "left_pr2arm_set_end_effector" / @b pr2_msgs::EndEffectorState : Desired pose of the left arm. (Should probably be a service)
+ - @b "left_pr2arm_pos" / @b std_msgs::PR2Arm : current joint angles of the left arm.
+ - @b "right_pr2arm_pos" / @b std_msgs::PR2Arm : current joint angles of the right arm
+ - @b "interpolate_step_size" / @b std_msgs::Float64 : step size for the linear interpolation. Should also be a service.
+ - @b "interpolate_wait_time" / @b std_msgs::Float64 : wait in wall clock time (and seconds) between successive interpolation commands. Useful for making the trajectory smooth. Should also be a service.
+
+ Publishes to (name / type):
+ - @b "cmd_leftarm_cartesian" / @b pr2_msgs::EndEffectorState : desired cartesian pose of the left arm. Legacy from interpolated_kinematic_controllers. Should be converted over to a service like the right arm. Probably doesn't work either.
+ - @b "rightarm_tooltip_cartesian" / @b pr2_msgs::EndEffectorState : Current pose of the end effector of the right arm.
+
+ <hr>
+
+ @section parameters ROS parameters
+
+ - None
+
+ <hr>
+
+ @section services ROS services
+
+ Advertises (name/type):
+ - @b "move_along_gripper" / @b pr2_kinematic_contollers::Float64Int32 : move end effector in the direction of the gripper through the specidifed distance. (Reaching motion)
+
+ Calls (name/type):
+ - @b "reset_IK_guess" / @b rosgazebo::VoidVoid : command to the initialize the joint array for the initial guess for solving the IK with the current joint angles.
+ - @b "set_rightarm_cartesian" / @b rosgazebo::MoveCartesian : desired end effector pose for the right arm.
+
+**/
+
+
+
#include <ros/node.h>
#include <std_msgs/PR2Arm.h>
@@ -26,239 +109,242 @@
class Pr2KinematicControllers : public ros::node
{
- public:
+ public:
- Pr2KinematicControllers(void) : ros::node("pr2_kinematic_controller")
- {
- // matches send.xml
- std::string pr2Content;
- get_param("robotdesc/pr2",pr2Content);
- pr2_kin.loadXMLString(pr2Content.c_str());
+ Pr2KinematicControllers(void) : ros::node("pr2_kinematic_controller")
+ {
+ // matches send.xml
+ std::string pr2Content;
+ get_param("robotdesc/pr2",pr2Content);
+ pr2_kin.loadXMLString(pr2Content.c_str());
- right_arm = pr2_kin.getSerialChain("rightArm");
+ right_arm = pr2_kin.getSerialChain("rightArm");
- step_size = 0.05;
- wait_time = 1;
- advertise<pr2_msgs::EndEffectorState>("cmd_leftarm_cartesian");
+ step_size = 0.05;
+ wait_time = 1;
+ advertise<pr2_msgs::EndEffectorState>("cmd_leftarm_cartesian");
- advertise<pr2_msgs::EndEffectorState>("rightarm_tooltip_cartesian"); // position of tip of right arm in cartesian coord.
- advertise_service("move_along_gripper", &Pr2KinematicControllers::moveAlongGripper);
+ 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); // configuration of left arm.
- subscribe("right_pr2arm_pos", rightArmPosMsg, &Pr2KinematicControllers::currentRightArmPos); // configuration of right arm.
+ 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); // 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);
- }
+ subscribe("interpolate_step_size", float64_msg, &Pr2KinematicControllers::setStepSize);
+ subscribe("interpolate_wait_time", float64_msg, &Pr2KinematicControllers::setWaitTime);
+ }
- void setWaitTime(void)
- {
- wait_time = float64_msg.data;
- }
+ void setWaitTime(void)
+ {
+ wait_time = float64_msg.data;
+ }
- void setStepSize(void)
- {
- step_size = float64_msg.data;
- }
+ void setStepSize(void)
+ {
+ step_size = float64_msg.data;
+ }
- void setRightEndEffector(void)
- {
- KDL::Frame f;
- for(int i = 0; i < 9; i++)
- f.M.data[i] = _rightEndEffectorGoal.rot[i];
+ void setRightEndEffector(void)
+ {
+ KDL::Frame f;
+ for(int i = 0; i < 9; i++)
+ f.M.data[i] = _rightEndEffectorGoal.rot[i];
- for(int i = 0; i < 3; i++)
- f.p.data[i] = _rightEndEffectorGoal.trans[i];
+ for(int i = 0; i < 3; i++)
+ f.p.data[i] = _rightEndEffectorGoal.trans[i];
- RunControlLoop(true, f, rightArmPosMsg);
- }
+ RunControlLoop(true, f, rightArmPosMsg);
+ }
- void setLeftEndEffector(void)
- {
- KDL::Frame f;
- for(int i = 0; i < 9; i++)
- f.M.data[i] = _leftEndEffectorGoal.rot[i];
+ void setLeftEndEffector(void)
+ {
+ KDL::Frame f;
+ for(int i = 0; i < 9; i++)
+ f.M.data[i] = _leftEndEffectorGoal.rot[i];
- for(int i = 0; i < 3; i++)
- f.p.data[i] = _leftEndEffectorGoal.trans[i];
+ for(int i = 0; i < 3; i++)
+ f.p.data[i] = _leftEndEffectorGoal.trans[i];
- RunControlLoop(false, f, leftArmPosMsg);
- }
+ RunControlLoop(false, f, leftArmPosMsg);
+ }
- void currentLeftArmPos(void)
- {
- }
+ void currentLeftArmPos(void)
+ {
+ }
- void currentRightArmPos(void)
- {
- 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;
+ void currentRightArmPos(void)
+ {
+ 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);
- }
+ Frame f;
+ right_arm->computeFK(q,f);
+ pr2_msgs::EndEffectorState efs;
+ KDL_to_EndEffectorStateMsg(f, efs);
+ publish("rightarm_tooltip_cartesian",efs);
+ }
- void KDL_to_EndEffectorStateMsg(const Frame& f, pr2_msgs::EndEffectorState &efs)
- {
- efs.set_rot_size(9);
- efs.set_trans_size(3);
- for(int i = 0; i < 9; i++)
- efs.rot[i] = f.M.data[i];
+ void KDL_to_EndEffectorStateMsg(const Frame& f, pr2_msgs::EndEffectorState &efs)
+ {
+ efs.set_rot_size(9);
+ efs.set_trans_size(3);
+ 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];
- }
+ for(int i = 0; i < 3; i++)
+ efs.trans[i] = f.p.data[i];
+ }
- void publishFrame(bool isRightArm, const Frame& f)
- {
- pr2_msgs::EndEffectorState efs;
- KDL_to_EndEffectorStateMsg(f, efs);
+ void publishFrame(bool isRightArm, const Frame& f)
+ {
+ pr2_msgs::EndEffectorState efs;
+ KDL_to_EndEffectorStateMsg(f, efs);
- if(isRightArm)
- publish("cmd_rightarm_cartesian",efs);
- else
- publish("cmd_leftarm_cartesian",efs);
- }
+ if(isRightArm)
+ {
+ printf("[pr2_kinematic_controllers/pr2_kinematic_controllers.cc] <publishFrame> For the right arm, SetRightArmCartesian should be used instead of publishFrame\nExiting..\n");
+ exit(0);
+ }
+ else
+ publish("cmd_leftarm_cartesian",efs);
+ }
- void RunControlLoop(bool isRightArm, const Frame& r, const std_msgs::PR2Arm& arm)
- {
- rosgazebo::VoidVoid::request req;
- rosgazebo::VoidVoid::response res;
+ void RunControlLoop(bool isRightArm, const Frame& r, const std_msgs::PR2Arm& arm)
+ {
+ rosgazebo::VoidVoid::request req;
+ rosgazebo::VoidVoid::response res;
- cout<<"RunControlLoop: rotation: "<<r.M<<"\n";
+ cout<<"RunControlLoop: rotation: "<<r.M<<"\n";
- if (ros::service::call("reset_IK_guess", req, res)==false)
- {
- 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("[pr2_kinematic_controllers] <pr2_kinematic_controllers.cpp> reset_IK_guess service failed.\nExiting..\n");
+ exit(0);
+ }
- JntArray q = JntArray(right_arm->num_joints_);
- q(0) = arm.turretAngle;
- q(1) = arm.shoulderLiftAngle;
- q(2) = arm.upperarmRollAngle;
- q(3) = arm.elbowAngle;
- q(4) = arm.forearmRollAngle;
- q(5) = arm.wristPitchAngle;
- q(6) = arm.wristRollAngle;
+ JntArray q = JntArray(right_arm->num_joints_);
+ q(0) = arm.turretAngle;
+ q(1) = arm.shoulderLiftAngle;
+ q(2) = arm.upperarmRollAngle;
+ q(3) = arm.elbowAngle;
+ q(4) = arm.forearmRollAngle;
+ q(5) = arm.wristPitchAngle;
+ q(6) = arm.wristRollAngle;
- Frame f;
- right_arm->computeFK(q,f);
- Vector start = f.p;
- Vector move = r.p-start;
- double dist = move.Norm();
- move = move/dist;
+ Frame f;
+ right_arm->computeFK(q,f);
+ Vector start = f.p;
+ Vector move = r.p-start;
+ double dist = move.Norm();
+ move = move/dist;
- RotationalInterpolation_SingleAxis rotInterpolater;
- rotInterpolater.SetStartEnd(f.M, r.M);
- double total_angle = rotInterpolater.Angle();
- // printf("Angle: %f\n", rotInterpolater.Angle());
+ RotationalInterpolation_SingleAxis rotInterpolater;
+ rotInterpolater.SetStartEnd(f.M, r.M);
+ double total_angle = rotInterpolater.Angle();
+ // printf("Angle: %f\n", rotInterpolater.Angle());
- Vector target;
- int nSteps = (int)(dist/step_size);
- double angle_step = total_angle/nSteps;
- 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));
+ Vector target;
+ int nSteps = (int)(dist/step_size);
+ double angle_step = total_angle/nSteps;
+ 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));
- if (isRightArm)
- reachable=SetRightArmCartesian(f); // services.
- else
- publishFrame(isRightArm, f); // old way of doing things. from interpolated_kinematic_controller.
+ if (isRightArm)
+ reachable=SetRightArmCartesian(f); // services.
+ else
+ publishFrame(isRightArm, f); // old way of doing things. from interpolated_kinematic_controller.
- usleep(wait_time*1e6);
- }
+ usleep(wait_time*1e6);
+ }
- f.p = r.p;
- f.M = r.M;
- if (isRightArm)
- reachable=SetRightArmCartesian(f); // services.
- else
- publishFrame(isRightArm, f); // old way of doing things. from interpolated_kinematic_controller.
+ f.p = r.p;
+ f.M = r.M;
+ 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");
- }
+ 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);
+ 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);
- }
+ 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;
- }
+ 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;
- }
+ 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_);
+ 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);
- }
+ 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:
+ private:
- pr2_msgs::EndEffectorState _leftEndEffectorGoal;
- pr2_msgs::EndEffectorState _rightEndEffectorGoal;
- std_msgs::PR2Arm leftArmPosMsg, rightArmPosMsg;
- double step_size;
- double wait_time;
- std_msgs::Float64 float64_msg;
-
- RobotKinematics pr2_kin;
- SerialChain *right_arm;
+ pr2_msgs::EndEffectorState _leftEndEffectorGoal;
+ pr2_msgs::EndEffectorState _rightEndEffectorGoal;
+ std_msgs::PR2Arm leftArmPosMsg, rightArmPosMsg;
+ double step_size;
+ double wait_time;
+ std_msgs::Float64 float64_msg;
+
+ RobotKinematics pr2_kin;
+ SerialChain *right_arm;
};
int main(int argc, char **argv)
{
- ros::init(argc, argv);
- Pr2KinematicControllers easy;
- easy.spin();
- easy.shutdown();
- return 0;
+ ros::init(argc, argv);
+ Pr2KinematicControllers easy;
+ easy.spin();
+ easy.shutdown();
+ return 0;
}
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-15 18:14:39 UTC (rev 3124)
+++ pkg/trunk/manip/pr2_kinematic_controllers/test/test_pr2_kin_con.cc 2008-08-15 18:39:28 UTC (rev 3125)
@@ -1,3 +1,34 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+// Author: Advait Jain
+
#include <ros/node.h>
#include <libpr2API/pr2API.h>
#include <pr2_msgs/EndEffectorState.h>
Modified: pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc
===================================================================
--- pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc 2008-08-15 18:14:39 UTC (rev 3124)
+++ pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc 2008-08-15 18:39:28 UTC (rev 3125)
@@ -1,5 +1,4 @@
/*
- * teleop_base_keyboard
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
@@ -11,7 +10,7 @@
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
- * * Neither the name of the <ORGANIZATION> nor the names of its
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
@@ -28,40 +27,42 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
+// Author: Advait Jain
+
/**
-
-@mainpage
-
-@htmlinclude manifest.html
-
-@b teleop_arm_keyboard teleoperates the arms of the PR2 by mapping
-key presses into joint angle set points.
-
-<hr>
-
-@section usage Usage
-@verbatim
-$ teleop_arm_keyboard [standard ROS args]
-@endverbatim
-
-Key mappings are printed to screen on startup.
-
-<hr>
-
-@section topic ROS topics
-
-Subscribes to (name/type):
-- None
-
-Publishes to (name / type):
-- @b "cmd_leftarmconfig"/PR2Arm : configuration of the left arm (all the joint angles); sent on every keypress.
-
-<hr>
-
-@section parameters ROS parameters
-
-- None
-
+
+ @mainpage
+
+ @htmlinclude manifest.html
+
+ @b teleop_arm_keyboard teleoperates the arms of the PR2 by mapping
+ key presses into joint angle set points.
+
+ <hr>
+
+ @section usage Usage
+ @verbatim
+ $ teleop_arm_keyboard [standard ROS args]
+ @endverbatim
+
+ Key mappings are printed to screen on startup.
+
+ <hr>
+
+ @section topic ROS topics
+
+ Subscribes to (name/type):
+ - None
+
+ Publishes to (name / type):
+ - @b "cmd_leftarmconfig"/PR2Arm : configuration of the left arm (all the joint angles); sent on every keypress.
+
+ <hr>
+
+ @section parameters ROS parameters
+
+ - None
+
**/
#include <termios.h>
@@ -123,173 +124,173 @@
class TArmK_Node : public ros::node
{
-private:
+ private:
std_msgs::PR2Arm cmd_leftarmconfig;
std_msgs::PR2Arm cmd_rightarmconfig;
-public:
- TArmK_Node() : ros::node("tarmk"), tf(*this, true)
- {
- // cmd_armconfig should probably be initialised
- // with the current joint angles of the arm rather
- // than zeros.
- this->cmd_leftarmconfig.turretAngle = 0;
- this->cmd_leftarmconfig.shoulderLiftAngle = 0;
- this->cmd_leftarmconfig.upperarmRollAngle = 0;
- this->cmd_leftarmconfig.elbowAngle = 0;
- this->cmd_leftarmconfig.forearmRollAngle = 0;
- this->cmd_leftarmconfig.wristPitchAngle = 0;
- this->cmd_leftarmconfig.wristRollAngle = 0;
- this->cmd_leftarmconfig.gripperForceCmd = 1000;
- this->cmd_leftarmconfig.gripperGapCmd = 0;
- this->cmd_rightarmconfig.turretAngle = 0;
- this->cmd_rightarmconfig.shoulderLiftAngle = 0;
- this->cmd_rightarmconfig.upperarmRollAngle = 0;
- this->cmd_rightarmconfig.elbowAngle = 0;
- this->cmd_rightarmconfig.forearmRollAngle = 0;
- this->cmd_rightarmconfig.wristPitchAngle = 0;
- this->cmd_rightarmconfig.wristRollAngle = 0;
- this->cmd_rightarmconfig.gripperForceCmd = 1000;
- this->cmd_rightarmconfig.gripperGapCmd = 0;
- advertise<std_msgs::PR2Arm>("cmd_leftarmconfig");
- advertise<std_msgs::PR2Arm>("cmd_rightarmconfig");
+ public:
+ TArmK_Node() : ros::node("tarmk"), tf(*this, true)
+ {
+ // cmd_armconfig should probably be initialised
+ // with the current joint angles of the arm rather
+ // than zeros.
+ this->cmd_leftarmconfig.turretAngle = 0;
+ this->cmd_leftarmconfig.shoulderLiftAngle = 0;
+ this->cmd_leftarmconfig.upperarmRollAngle = 0;
+ this->cmd_leftarmconfig.elbowAngle = 0;
+ this->cmd_leftarmconfig.forearmRollAngle = 0;
+ this->cmd_leftarmconfig.wristPitchAngle = 0;
+ this->cmd_leftarmconfig.wristRollAngle = 0;
+ this->cmd_leftarmconfig.gripperForceCmd = 1000;
+ this->cmd_leftarmconfig.gripperGapCmd = 0;
+ this->cmd_rightarmconfig.turretAngle = 0;
+ this->cmd_rightarmconfig.shoulderLiftAngle = 0;
+ this->cmd_rightarmconfig.upperarmRollAngle = 0;
+ this->cmd_rightarmconfig.elbowAngle = 0;
+ this->cmd_rightarmconfig.forearmRollAngle = 0;
+ this->cmd_rightarmconfig.wristPitchAngle = 0;
+ this->cmd_rightarmconfig.wristRollAngle = 0;
+ this->cmd_rightarmconfig.gripperForceCmd = 1000;
+ this->cmd_rightarmconfig.gripperGapCmd = 0;
+ advertise<std_msgs::PR2Arm>("cmd_leftarmconfig");
+ advertise<std_msgs::PR2Arm>("cmd_rightarmconfig");
- _leftInit = false;
- _rightInit = false;
-
- //for getting positions
- subscribe("left_pr2arm_pos", leftArmPosMsg, &TArmK_Node::leftArmPosReceived);
- subscribe("right_pr2arm_pos", rightArmPosMsg, &TArmK_Node::rightArmPosReceived);
- }
+ _leftInit = false;
+ _rightInit = false;
+
+ //for getting positions
+ subscribe("left_pr2arm_pos", leftArmPosMsg, &TArmK_Node::leftArmPosReceived);
+ subscribe("right_pr2arm_pos", rightArmPosMsg, &TArmK_Node::rightArmPosReceived);
+ }
~TArmK_Node() { }
- void printCurrentJointValues() {
- std::cout << "Left joint angles:" << std::endl;
- std::cout << "turretAngle " << leftArmPosMsg.turretAngle << std::endl;
- std::cout << "shoulderLiftAngle " << leftArmPosMsg.shoulderLiftAngle << std::endl;
- std::cout << "upperarmRollAngle " << leftArmPosMsg.upperarmRollAngle << std::endl;
- std::cout << "elbowAngle " << leftArmPosMsg.elbowAngle << std::endl;
- std::cout << "forearmRollAngle " << leftArmPosMsg.forearmRollAngle << std::endl;
- std::cout << "wristPitchAngle " << leftArmPosMsg.wristPitchAngle << std::endl;
- std::cout << "wristRollAngle " << leftArmPosMsg.wristRollAngle << std::endl;
- std::cout << "gripperForceCmd " << leftArmPosMsg.gripperForceCmd << std::endl;
- std::cout << "gripperGapCmd " << leftArmPosMsg.gripperGapCmd << std::endl;
+ void printCurrentJointValues() {
+ std::cout << "Left joint angles:" << std::endl;
+ std::cout << "turretAngle " << leftArmPosMsg.turretAngle << std::endl;
+ std::cout << "shoulderLiftAngle " << leftArmPosMsg.shoulderLiftAngle << std::endl;
+ std::cout << "upperarmRollAngle " << leftArmPosMsg.upperarmRollAngle << std::endl;
+ std::cout << "elbowAngle " << leftArmPosMsg.elbowAngle << std::endl;
+ std::cout << "forearmRollAngle " << leftArmPosMsg.forearmRollAngle << std::endl;
+ std::cout << "wristPitchAngle " << leftArmPosMsg.wristPitchAngle << std::endl;
+ std::cout << "wristRollAngle " << leftArmPosMsg.wristRollAngle << std::endl;
+ std::cout << "gripperForceCmd " << leftArmPosMsg.gripperForceCmd << std::endl;
+ std::cout << "gripperGapCmd " << leftArmPosMsg.gripperGapCmd << std::endl;
- std::cout << "Right joint angles:" << std::endl;
- std::cout << "turretAngle " << rightArmPosMsg.turretAngle << std::endl;
- std::cout << "shoulderLiftAngle " << rightArmPosMsg.shoulderLiftAngle << std::endl;
- std::cout << "upperarmRollAngle " << rightArmPosMsg.upperarmRollAngle << std::endl;
- std::cout << "elbowAngle " << rightArmPosMsg.elbowAngle << std::endl;
- std::cout << "forearmRollAngle " << rightArmPosMsg.forearmRollAngle << std::endl;
- std::cout << "wristPitchAngle " << rightArmPosMsg.wristPitchAngle << std::endl;
- std::cout << "wristRollAngle " << rightArmPosMsg.wristRollAngle << std::endl;
- std::cout << "gripperForceCmd " << rightArmPosMsg.gripperForceCmd << std::endl;
- std::cout << "gripperGapCmd " << rightArmPosMsg.gripperGapCmd << std::endl;
- }
+ std::cout << "Right joint angles:" << std::endl;
+ std::cout << "turretAngle " << rightArmPosMsg.turretAngle << std::endl;
+ std::cout << "shoulderLiftAngle " << rightArmPosMsg.shoulderLiftAngle << std::endl;
+ std::cout << "upperarmRollAngle " << rightArmPosMsg.upperarmRollAngle << std::endl;
+ std::cout << "elbowAngle " << rightArmPosMsg.elbowAngle << std::endl;
+ std::cout << "forearmRollAngle " << rightArmPosMsg.forearmRollAngle << std::endl;
+ std::cout << "wristPitchAngle " << rightArmPosMsg.wristPitchAngle << std::endl;
+ std::cout << "wristRollAngle " << rightArmPosMsg.wristRollAngle << std::endl;
+ std::cout << "gripperForceCmd " << rightArmPosMsg.gripperForceCmd << std::endl;
+ std::cout << "gripperGapCmd " << rightArmPosMsg.gripperGapCmd << std::endl;
+ }
- void printCurrentEndEffectorWorldCoord() {
- libTF::TFPose aPose;
- aPose.x = 0.0;
- aPose.y = 0.0;
- aPose.z = 0.0;
- aPose.roll = 0;
- aPose.pitch = 0;
- aPose.yaw = 0;
- aPose.time = 0;
- aPose.frame = tf.lookup("FRAMEID_ARM_R_HAND");
+ void printCurrentEndEffectorWorldCoord() {
+ libTF::TFPose aPose;
+ aPose.x = 0.0;
+ aPose.y = 0.0;
+ aPose.z = 0.0;
+ aPose.roll = 0;
+ aPose.pitch = 0;
+ aPose.yaw = 0;
+ aPose.time = 0;
+ aPose.frame = tf.lookup("FRAMEID_ARM_R_HAND");
- libTF::TFPose inOdomFrame = tf.transformPose("FRAMEID_ODOM", aPose);
+ libTF::TFPose inOdomFrame = tf.transformPose("FRAMEID_ODOM", aPose);
- std::cout << "In odom frame x " << inOdomFrame.x << std::endl;
- std::cout << "In odom frame y " << inOdomFrame.y << std::endl;
- std::cout << "In odom frame z " << inOdomFrame.z << std::endl;
- }
+ std::cout << "In odom frame x " << inOdomFrame.x << std::endl;
+ std::cout << "In odom frame y " << inOdomFrame.y << std::endl;
+ std::cout << "In odom frame z " << inOdomFrame.z << std::endl;
+ }
- void printCurrentEndEffectorShoulderCoord() {
- libTF::TFPose aPose;
- aPose.x = .64;
- aPose.y = -.37;
- aPose.z = 1.76;
- aPose.roll = 0;
- aPose.pitch = 0;
- aPose.yaw = 0;
- aPose.time = 0;
- aPose.frame = tf.lookup("FRAMEID_ODOM");
+ void printCurrentEndEffectorShoulderCoord() {
+ libTF::TFPose aPose;
+ aPose.x = .64;
+ aPose.y = -.37;
+ aPose.z = 1.76;
+ aPose.roll = 0;
+ aPose.pitch = 0;
+ aPose.yaw = 0;
+ aPose.time = 0;
+ aPose.frame = tf.lookup("FRAMEID_ODOM");
- libTF::TFPose inOdomFrame = tf.transformPose("FRAMEID_ARM_R_SHOULDER", aPose);
+ libTF::TFPose inOdomFrame = tf.transformPose("FRAMEID_ARM_R_SHOULDER", aPose);
- std::cout << "In shoulder frame x " << inOdomFrame.x << std::endl;
- std::cout << "In shoulder frame y " << inOdomFrame.y <<...
[truncated message content] |
