Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2354] pkg/trunk
|
From: <adv...@us...> - 2008-07-30 23:38:01
|
Revision: 2354
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2354&view=rev
Author: advaitjain
Date: 2008-07-30 23:38:02 +0000 (Wed, 30 Jul 2008)
Log Message:
-----------
changed reset_IK_guess message to a service -- RosGazeboNode
Modified Paths:
--------------
pkg/trunk/simulators/rosgazebo/CMakeLists.txt
pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
pkg/trunk/simulators/rosgazebo/manifest.xml
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
Added Paths:
-----------
pkg/trunk/manip/pr2_kinematic_controllers/
pkg/trunk/manip/pr2_kinematic_controllers/CMakeLists.txt
pkg/trunk/manip/pr2_kinematic_controllers/Makefile
pkg/trunk/manip/pr2_kinematic_controllers/bin/
pkg/trunk/manip/pr2_kinematic_controllers/include/
pkg/trunk/manip/pr2_kinematic_controllers/manifest.xml
pkg/trunk/manip/pr2_kinematic_controllers/src/
pkg/trunk/manip/pr2_kinematic_controllers/src/pr2_kinematic_controllers.cc
pkg/trunk/manip/pr2_kinematic_controllers/srv/
pkg/trunk/manip/pr2_kinematic_controllers/srv/Float32Int32.srv
pkg/trunk/manip/pr2_kinematic_controllers/test/
pkg/trunk/manip/pr2_kinematic_controllers/test/test_pr2_kin_con.cc
pkg/trunk/simulators/rosgazebo/srv/
pkg/trunk/simulators/rosgazebo/srv/VoidVoid.srv
Added: pkg/trunk/manip/pr2_kinematic_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/manip/pr2_kinematic_controllers/CMakeLists.txt (rev 0)
+++ pkg/trunk/manip/pr2_kinematic_controllers/CMakeLists.txt 2008-07-30 23:38:02 UTC (rev 2354)
@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(pr2_kinematic_controllers)
+
+gensrv()
+add_definitions(-Wall)
+include_directories(srv/cpp)
+
+rospack_add_executable(bin/pr2_kinematic_controllers src/pr2_kinematic_controllers.cc)
+rospack_add_executable(bin/test_pr2_kin_con test/test_pr2_kin_con.cc)
+
+
Added: pkg/trunk/manip/pr2_kinematic_controllers/Makefile
===================================================================
--- pkg/trunk/manip/pr2_kinematic_controllers/Makefile (rev 0)
+++ pkg/trunk/manip/pr2_kinematic_controllers/Makefile 2008-07-30 23:38:02 UTC (rev 2354)
@@ -0,0 +1,2 @@
+include $(shell rospack find mk)/cmake.mk
+
Added: pkg/trunk/manip/pr2_kinematic_controllers/manifest.xml
===================================================================
--- pkg/trunk/manip/pr2_kinematic_controllers/manifest.xml (rev 0)
+++ pkg/trunk/manip/pr2_kinematic_controllers/manifest.xml 2008-07-30 23:38:02 UTC (rev 2354)
@@ -0,0 +1,17 @@
+<package>
+ <description brief='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="pr2_gazebo"/>
+ <export>
+ <cpp cflags="-I${prefix}/srv/cpp" />
+ </export>
+</package>
Added: pkg/trunk/manip/pr2_kinematic_controllers/src/pr2_kinematic_controllers.cc
===================================================================
--- pkg/trunk/manip/pr2_kinematic_controllers/src/pr2_kinematic_controllers.cc (rev 0)
+++ pkg/trunk/manip/pr2_kinematic_controllers/src/pr2_kinematic_controllers.cc 2008-07-30 23:38:02 UTC (rev 2354)
@@ -0,0 +1,194 @@
+#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/Float32Int32.h>
+
+#include <libpr2API/pr2API.h>
+
+#include <kdl/rotational_interpolation_sa.hpp>
+#include <rosgazebo/VoidVoid.h>
+
+
+using namespace std_msgs;
+using namespace PR2;
+using namespace KDL;
+
+
+class Pr2KinematicControllers : public ros::node
+{
+ public:
+
+ Pr2KinematicControllers(void) : ros::node("pr2_kinematic_controller")
+ {
+ step_size = 0.05;
+ 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.
+
+ 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("interpolate_step_size", float64_msg, &Pr2KinematicControllers::setStepSize);
+ subscribe("interpolate_wait_time", float64_msg, &Pr2KinematicControllers::setWaitTime);
+ }
+
+ void setWaitTime(void)
+ {
+ wait_time = 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];
+
+ for(int i = 0; i < 3; i++)
+ f.p.data[i] = _rightEndEffectorGoal.trans[i];
+
+ RunControlLoop(true, f, rightArmPosMsg);
+ }
+
+ 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];
+
+ RunControlLoop(false, f, leftArmPosMsg);
+ }
+
+
+ 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
+ }
+
+ void publishFrame(bool isRightArm, const Frame& f)
+ {
+ 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;
+ if(isRightArm)
+ publish("cmd_rightarm_cartesian",efs);
+ else
+ publish("cmd_leftarm_cartesian",efs);
+ }
+
+ 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
+ {
+ printf("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);
+
+ 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;
+ pr2_kin.FK(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();
+ // printf("Angle: %f\n", rotInterpolater.Angle());
+
+ Vector target;
+ int nSteps = (int)(dist/step_size);
+ double angle_step = total_angle/nSteps;
+ for(int i=0;i<nSteps;i++)
+ {
+ 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);
+ usleep(wait_time*1e6);
+ }
+ f.p = r.p;
+ f.M = r.M;
+ publishFrame(isRightArm, f);
+ }
+
+ 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;
+
+};
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+ Pr2KinematicControllers easy;
+ easy.spin();
+ easy.shutdown();
+ return 0;
+}
+
Added: pkg/trunk/manip/pr2_kinematic_controllers/srv/Float32Int32.srv
===================================================================
--- pkg/trunk/manip/pr2_kinematic_controllers/srv/Float32Int32.srv (rev 0)
+++ pkg/trunk/manip/pr2_kinematic_controllers/srv/Float32Int32.srv 2008-07-30 23:38:02 UTC (rev 2354)
@@ -0,0 +1,3 @@
+float32 f
+---
+int32 i
Added: pkg/trunk/manip/pr2_kinematic_controllers/test/test_pr2_kin_con.cc
===================================================================
--- pkg/trunk/manip/pr2_kinematic_controllers/test/test_pr2_kin_con.cc (rev 0)
+++ pkg/trunk/manip/pr2_kinematic_controllers/test/test_pr2_kin_con.cc 2008-07-30 23:38:02 UTC (rev 2354)
@@ -0,0 +1,47 @@
+#include <ros/node.h>
+#include <libpr2API/pr2API.h>
+#include <pr2_msgs/EndEffectorState.h>
+
+using namespace KDL;
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+
+ ros::node mynode("test_inter+kin_con");
+ mynode.advertise<pr2_msgs::EndEffectorState>("right_pr2arm_set_end_effector");
+
+ //In shoulder frame x 0.562689
+ //In shoulder frame y -0.367447
+ //In shoulder frame z -0.369594
+ Rotation r = Rotation::RotZ(DTOR(0));
+ Vector v(.562689,-.367447,-.369594);
+
+ std::cout << " rot: " << std::endl;
+ std::cout << r.data[0] << std::endl;
+ std::cout << r.data[1] << std::endl;
+ std::cout << r.data[2] << std::endl;
+ std::cout << r.data[3] << std::endl;
+ std::cout << r.data[4] << std::endl;
+ std::cout << r.data[5] << std::endl;
+ std::cout << r.data[6] << std::endl;
+ std::cout << r.data[7] << std::endl;
+ std::cout << r.data[8] << std::endl;
+
+ sleep(1);
+
+ 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];
+
+
+ mynode.publish("right_pr2arm_set_end_effector",efs);
+ sleep(1);
+ mynode.shutdown();
+ return 0;
+}
Modified: pkg/trunk/simulators/rosgazebo/CMakeLists.txt
===================================================================
--- pkg/trunk/simulators/rosgazebo/CMakeLists.txt 2008-07-30 23:35:27 UTC (rev 2353)
+++ pkg/trunk/simulators/rosgazebo/CMakeLists.txt 2008-07-30 23:38:02 UTC (rev 2354)
@@ -3,6 +3,8 @@
rospack(rosgazebo)
genmsg()
include_directories(msg/cpp)
+gensrv()
+include_directories(srv/cpp)
add_definitions(-Wall)
rospack_add_library(RosGazeboNode src/RosGazeboNode.cpp)
Modified: pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
===================================================================
--- pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-07-30 23:35:27 UTC (rev 2353)
+++ pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-07-30 23:38:02 UTC (rev 2354)
@@ -65,6 +65,9 @@
#include <time.h>
+// service
+#include <rosgazebo/VoidVoid.h>
+
// Our node
class RosGazeboNode : public ros::node
{
@@ -157,7 +160,7 @@
// Message callback which sets pr2_kin.q_IK_guess to the current manipulator configuration.
// July 24, 2008 - Advait - only right arm is supported
- void reset_IK_guess();
+ bool reset_IK_guess(rosgazebo::VoidVoid::request &req, rosgazebo::VoidVoid::response &res);
// laser range data
float ranges[GZ_LASER_MAX_RANGES];
Modified: pkg/trunk/simulators/rosgazebo/manifest.xml
===================================================================
--- pkg/trunk/simulators/rosgazebo/manifest.xml 2008-07-30 23:35:27 UTC (rev 2353)
+++ pkg/trunk/simulators/rosgazebo/manifest.xml 2008-07-30 23:38:02 UTC (rev 2354)
@@ -23,6 +23,6 @@
<depend package="gazebo_hardware" />
<depend package="gazebo_sensors" />
<export>
- <cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lRosGazeboNode"/>
+ <cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp -I${prefix}/srv/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lRosGazeboNode"/>
</export>
</package>
Modified: pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
===================================================================
--- pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-30 23:35:27 UTC (rev 2353)
+++ pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-30 23:38:02 UTC (rev 2354)
@@ -126,9 +126,10 @@
this->lock.unlock();
}
-void RosGazeboNode::reset_IK_guess()
+bool RosGazeboNode::reset_IK_guess(rosgazebo::VoidVoid::request &req, rosgazebo::VoidVoid::response &res)
{
this->PR2Copy->GetArmJointPositionCmd(PR2::PR2_RIGHT_ARM, *(this->PR2Copy->pr2_kin.q_IK_guess));
+ return true;
}
void RosGazeboNode::cmd_rightarmcartesianReceived() {
@@ -354,8 +355,10 @@
subscribe("cmd_rightarmconfig", rightarm, &RosGazeboNode::cmd_rightarmconfigReceived);
subscribe("cmd_leftarm_cartesian", cmd_leftarmcartesian, &RosGazeboNode::cmd_leftarmcartesianReceived);
subscribe("cmd_rightarm_cartesian", cmd_rightarmcartesian, &RosGazeboNode::cmd_rightarmcartesianReceived);
- subscribe("reset_IK_guess", empty_msg, &RosGazeboNode::reset_IK_guess);
+ //------ services ----------
+ advertise_service("reset_IK_guess", &RosGazeboNode::reset_IK_guess);
+
return(0);
}
Added: pkg/trunk/simulators/rosgazebo/srv/VoidVoid.srv
===================================================================
--- pkg/trunk/simulators/rosgazebo/srv/VoidVoid.srv (rev 0)
+++ pkg/trunk/simulators/rosgazebo/srv/VoidVoid.srv 2008-07-30 23:38:02 UTC (rev 2354)
@@ -0,0 +1,3 @@
+int32 i
+---
+int32 i
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
