Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2214] pkg/trunk
|
From: <adv...@us...> - 2008-07-28 23:38:25
|
Revision: 2214
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2214&view=rev
Author: advaitjain
Date: 2008-07-28 23:38:31 +0000 (Mon, 28 Jul 2008)
Log Message:
-----------
Changed interpolation and IK to save the previous guess
within the pr2_kinematics class. This removes the need
for passing an initial guess configuration.
Also changed interpolated_kinematic_controller to use result
of previous IK as the new guess rather than reading the joint
angles.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2API/CMakeLists.txt
pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pick_object.cpp
pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pr2_kin_kdl.cpp
pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
pkg/trunk/util/kinematics/interpolated_kinematic_controller/src/interpolated_kinematic_controller.cc
pkg/trunk/util/kinematics/libKDL/CMakeLists.txt
pkg/trunk/util/kinematics/libKDL/include/libKDL/kdl_kinematics.h
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/
pkg/trunk/drivers/robot/pr2/libpr2API/src/old/kinematic_controllers.cpp
pkg/trunk/drivers/robot/pr2/libpr2API/src/old/test_kin_controllers.cpp
Removed Paths:
-------------
pkg/trunk/drivers/robot/pr2/libpr2API/src/kinematic_controllers.cpp
pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_kin_controllers.cpp
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/CMakeLists.txt 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/CMakeLists.txt 2008-07-28 23:38:31 UTC (rev 2214)
@@ -10,8 +10,11 @@
target_link_libraries(torque_test pr2API)
rospack_add_executable(pick_object src/test/pick_object.cpp)
target_link_libraries(pick_object pr2API)
-rospack_add_executable(test_kin_controllers src/test/test_kin_controllers.cpp src/kinematic_controllers.cpp)
-target_link_libraries(test_kin_controllers pr2API)
+
+# kinematic_controllers is now deprecated.
+#rospack_add_executable(test_kin_controllers src/test/test_kin_controllers.cpp src/kinematic_controllers.cpp)
+#target_link_libraries(test_kin_controllers pr2API)
+
rospack_add_executable(pr2_kin_kdl src/test/pr2_kin_kdl.cpp)
target_link_libraries(pr2_kin_kdl pr2API)
rospack_add_executable(test_FK src/test/test_FK.cpp)
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-07-28 23:38:31 UTC (rev 2214)
@@ -412,7 +412,12 @@
// KDL version of SetArmCartesianPosition
- public: PR2_ERROR_CODE SetArmCartesianPosition(PR2_MODEL_ID id, const KDL::Frame &f,const KDL::JntArray &q_init, KDL::JntArray &q_out);
+ public: PR2_ERROR_CODE SetArmCartesianPosition(PR2_MODEL_ID id, const KDL::Frame &f);
+ /*
+ * Preserved right now (July 28,2008) for compatibility with controller code.
+ * We might want to get rid of this later. -- Advait
+ */
+ PR2_ERROR_CODE SetArmCartesianPosition(PR2_MODEL_ID id, const KDL::Frame &f, const KDL::JntArray &q_init, KDL::JntArray &q_out);
/*! \fn
\brief Get the commanded position and speed for the end-effector
Deleted: pkg/trunk/drivers/robot/pr2/libpr2API/src/kinematic_controllers.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/kinematic_controllers.cpp 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/kinematic_controllers.cpp 2008-07-28 23:38:31 UTC (rev 2214)
@@ -1,69 +0,0 @@
-/*
- Higher level controllers for the arm.
- I don't want to add to libpr2API and proper place for this code is yet to
- be decided.
-
-
-*/
-
-#include "kinematic_controllers.h"
-#include <kdl/rotational_interpolation_sa.hpp>
-
-#include <sys/time.h>
-#include <unistd.h>
-
-using namespace KDL;
-
-kinematic_controllers::kinematic_controllers()
-{
- this->myPR2 = new PR2::PR2Robot();
-}
-
-void kinematic_controllers::init()
-{
- this->myPR2->InitializeRobot();
-}
-
-void kinematic_controllers::go(KDL::Vector p, KDL::Rotation r, double step_size)
-{
- JntArray q = JntArray(this->myPR2->pr2_kin.nJnts);
- struct timeval t0, t1;
- gettimeofday(&t0,NULL);
-
- this->myPR2->GetArmJointPositionCmd(PR2::PR2_RIGHT_ARM, q);
- cout<<"current joint angles:"<<q<<endl;
-
- Frame f;
- this->myPR2->pr2_kin.FK(q,f);
- Vector start = f.p;
- Vector move = p-start;
- double dist = move.Norm();
- move = move/dist;
-//cout << "Start:"<<start<<", End:"<<p<<", dist: "<<dist<<", direction: "<<move<<endl;
-
- RotationalInterpolation_SingleAxis rotInterpolater;
- rotInterpolater.SetStartEnd(f.M, r);
- 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));
- this->myPR2->SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f,q,q);
- }
-
- f.p = p;
- f.M = r;
- this->myPR2->SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f,q,q);
- gettimeofday(&t1,NULL);
- double time_taken = (t1.tv_sec*1000000+t1.tv_usec - (t0.tv_sec*1000000+t0.tv_usec))/1000.;
- printf("Time taken to go: %f ms\n", time_taken);
-
-}
-
-
-
Copied: pkg/trunk/drivers/robot/pr2/libpr2API/src/old/kinematic_controllers.cpp (from rev 2066, pkg/trunk/drivers/robot/pr2/libpr2API/src/kinematic_controllers.cpp)
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/old/kinematic_controllers.cpp (rev 0)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/old/kinematic_controllers.cpp 2008-07-28 23:38:31 UTC (rev 2214)
@@ -0,0 +1,69 @@
+/*
+ Higher level controllers for the arm.
+ I don't want to add to libpr2API and proper place for this code is yet to
+ be decided.
+
+
+*/
+
+#include "kinematic_controllers.h"
+#include <kdl/rotational_interpolation_sa.hpp>
+
+#include <sys/time.h>
+#include <unistd.h>
+
+using namespace KDL;
+
+kinematic_controllers::kinematic_controllers()
+{
+ this->myPR2 = new PR2::PR2Robot();
+}
+
+void kinematic_controllers::init()
+{
+ this->myPR2->InitializeRobot();
+}
+
+void kinematic_controllers::go(KDL::Vector p, KDL::Rotation r, double step_size)
+{
+ JntArray q = JntArray(this->myPR2->pr2_kin.nJnts);
+ struct timeval t0, t1;
+ gettimeofday(&t0,NULL);
+
+ this->myPR2->GetArmJointPositionCmd(PR2::PR2_RIGHT_ARM, q);
+ cout<<"current joint angles:"<<q<<endl;
+
+ Frame f;
+ this->myPR2->pr2_kin.FK(q,f);
+ Vector start = f.p;
+ Vector move = p-start;
+ double dist = move.Norm();
+ move = move/dist;
+//cout << "Start:"<<start<<", End:"<<p<<", dist: "<<dist<<", direction: "<<move<<endl;
+
+ RotationalInterpolation_SingleAxis rotInterpolater;
+ rotInterpolater.SetStartEnd(f.M, r);
+ 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));
+ this->myPR2->SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f,q,q);
+ }
+
+ f.p = p;
+ f.M = r;
+ this->myPR2->SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f,q,q);
+ gettimeofday(&t1,NULL);
+ double time_taken = (t1.tv_sec*1000000+t1.tv_usec - (t0.tv_sec*1000000+t0.tv_usec))/1000.;
+ printf("Time taken to go: %f ms\n", time_taken);
+
+}
+
+
+
Copied: pkg/trunk/drivers/robot/pr2/libpr2API/src/old/test_kin_controllers.cpp (from rev 2066, pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_kin_controllers.cpp)
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/old/test_kin_controllers.cpp (rev 0)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/old/test_kin_controllers.cpp 2008-07-28 23:38:31 UTC (rev 2214)
@@ -0,0 +1,133 @@
+
+#include "kinematic_controllers.h"
+#include <math.h>
+
+using namespace KDL;
+using namespace PR2;
+using namespace std;
+
+
+#include <termios.h>
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+int kfd = 0;
+struct termios cooked, raw;
+
+kinematic_controllers myKinCon;
+
+void top();
+void object_pose();
+void go_down();
+void close_gripper();
+void open_gripper();
+
+
+void keyboardLoop()
+{
+ 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);
+
+ puts("Reading from keyboard");
+ puts("---------------------------");
+ puts("---------------------------");
+
+ for(;;)
+ {
+ // get the next event from the keyboard
+ if(read(kfd, &c, 1) < 0)
+ {
+ perror("read():");
+ exit(-1);
+ }
+
+ switch(c)
+ {
+ case '1':
+ printf("You pressed 1\n");
+ open_gripper();
+ top();
+ break;
+ case '2':
+ printf("You pressed 2\n");
+ object_pose();
+ break;
+ case '3':
+ printf("You pressed 3\n");
+ go_down();
+ break;
+ case '4':
+ printf("You pressed 4\n");
+ close_gripper();
+ break;
+ case '5':
+ printf("You pressed 4\n");
+ object_pose();
+ break;
+ case '6':
+ printf("You pressed 4\n");
+ top();
+ break;
+ default:
+ printf("You pressed Something Else\n");
+ break;
+ }
+ myKinCon.myPR2->hw.UpdateHW();
+ }
+}
+
+void top()
+{
+ Rotation r = Rotation::RotZ(DTOR(0));
+ Vector v(0.3,-0.2,0.56);
+ myKinCon.go(v,r,0.05);
+}
+
+void object_pose()
+{
+ Rotation r = Rotation::RotZ(DTOR(90));
+ Vector v(0.578,0.01,0.06);
+ myKinCon.go(v,r,0.05);
+}
+
+void go_down()
+{
+ Rotation r = Rotation::RotZ(DTOR(90))*Rotation::RotY(DTOR(30));
+ Vector v(0.578,0.01,-0.01);
+ myKinCon.go(v,r,0.05);
+}
+
+void close_gripper()
+{
+ myKinCon.myPR2->hw.CloseGripper(PR2::PR2_RIGHT_GRIPPER, 0.0, 10000);
+}
+
+void open_gripper()
+{
+ myKinCon.myPR2->hw.CloseGripper(PR2::PR2_RIGHT_GRIPPER, 0.15, 500);
+}
+
+void init_robot()
+{
+ myKinCon.init();
+}
+
+
+
+int main(int argc, char **argv)
+{
+ init_robot();
+ keyboardLoop();
+ return 0;
+}
+
+
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-07-28 23:38:31 UTC (rev 2214)
@@ -149,7 +149,10 @@
return PR2_ALL_OK;
}
-
+/*
+ * Preserved right now (July 28,2008) for compatibility with controller code.
+ * We might want to get rid of this later. -- Advait
+ */
PR2_ERROR_CODE PR2Robot::SetArmCartesianPosition(PR2_MODEL_ID id, const KDL::Frame &f, const KDL::JntArray &q_init, KDL::JntArray &q_out)
{
// KDL::JntArray q_init = KDL::JntArray(this->pr2_kin.nJnts);
@@ -177,6 +180,36 @@
return PR2_ALL_OK;
}
+PR2_ERROR_CODE PR2Robot::SetArmCartesianPosition(PR2_MODEL_ID id, const KDL::Frame &f)
+{
+ if (this->pr2_kin.IK(f) == true)
+ {
+// cout<<"IK guess:"<<*this->pr2_kin.q_IK_guess<<endl;
+// cout<<"IK result:"<<*this->pr2_kin.q_IK_result<<endl;
+ }
+ else
+ cout<<"Could not compute Inv Kin."<<endl;
+
+ //------ checking that IK returned a valid soln -----
+ KDL::Frame f_ik;
+ if (this->pr2_kin.FK(*this->pr2_kin.q_IK_result,f_ik))
+ {
+ // cout<<"End effector after IK:"<<f_ik<<endl;
+ }
+ else
+ cout<<"Could not compute Fwd Kin. (After IK)"<<endl;
+
+ for(int ii = 0; ii < 7; ii++)
+ hw.SetJointServoCmd((PR2::PR2_JOINT_ID) (JointStart[id]+ii),(*this->pr2_kin.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.
+
+
+ return PR2_ALL_OK;
+}
+
PR2_ERROR_CODE PR2Robot::SetArmCartesianPosition(PR2_MODEL_ID id, NEWMAT::Matrix g)
{
NEWMAT::Matrix theta(8,8);
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pick_object.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pick_object.cpp 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pick_object.cpp 2008-07-28 23:38:31 UTC (rev 2214)
@@ -18,10 +18,10 @@
struct termios cooked, raw;
PR2::PR2Robot myPR2;
-JntArray object_pose();
+void object_pose();
void open_gripper();
void close_gripper();
-void go_down(const JntArray &guess);
+void go_down();
void keyboardLoop()
@@ -40,7 +40,6 @@
puts("---------------------------");
puts("---------------------------");
- JntArray q = JntArray(myPR2.pr2_kin.nJnts);
for(;;)
{
// get the next event from the keyboard
@@ -58,11 +57,11 @@
break;
case '2':
printf("You pressed 2\n");
- q = object_pose();
+ object_pose();
break;
case '3':
printf("You pressed 3\n");
- go_down(q);
+ go_down();
break;
case '4':
printf("You pressed 4\n");
@@ -91,12 +90,13 @@
Frame f;
myPR2.pr2_kin.FK(pr2_config,f);
// send command to robot
- JntArray q_init = JntArray(myPR2.pr2_kin.nJnts);
- q_init(0) = 0.0, q_init(1) = 0.0, q_init(2) = 0.0, q_init(3) = 0.0;
- q_init(4) = 0.0, q_init(5) = 0.0, q_init(6) = 0.0;
- JntArray q_out = JntArray(myPR2.pr2_kin.nJnts);
- myPR2.SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f,q_init,q_out);
+ (*myPR2.pr2_kin.q_IK_guess)(0) = 0.0, (*myPR2.pr2_kin.q_IK_guess)(1) = 0.0, (*myPR2.pr2_kin.q_IK_guess)(2) = 0.0;
+ (*myPR2.pr2_kin.q_IK_guess)(3) = 0.0, (*myPR2.pr2_kin.q_IK_guess)(4) = 0.0, (*myPR2.pr2_kin.q_IK_guess)(5) = 0.0;
+ (*myPR2.pr2_kin.q_IK_guess)(6) = 0.0;
+
+ myPR2.SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f);
+
}
void close_gripper()
@@ -109,16 +109,17 @@
myPR2.hw.CloseGripper(PR2::PR2_RIGHT_GRIPPER, 0.15, 500);
}
-void go_down(const JntArray &guess)
+void go_down()
{
+ // called after object_pose so q_IK_guess should already be correct.
Rotation r = Rotation::RotZ(DTOR(90));
Vector v(0.568,0.01,-0.01);
Frame f(r,v);
- JntArray q_out = JntArray(myPR2.pr2_kin.nJnts);
- myPR2.SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f,guess,q_out);
+
+ myPR2.SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f);
}
-JntArray object_pose()
+void object_pose()
{
Rotation r = Rotation::RotZ(DTOR(90));
@@ -126,41 +127,16 @@
Vector v(0.568,0.01,0.06);
Frame f(r,v);
- JntArray q_init = JntArray(myPR2.pr2_kin.nJnts);
- q_init(0) = DTOR(-30); // turret
- q_init(1) = DTOR(-20); // shoulder pitch
- q_init(2) = DTOR(60); // shoulder roll
- q_init(3) = DTOR(60); // elbow pitch
- q_init(4) = DTOR(20); // elbow roll
- q_init(5) = DTOR(30); // wrist pitch
- q_init(6) = 0.0; // wrist roll
-// myPR2.pr2_kin.FK(q_init,f);
-// cout<<"end effector frame: "<<f<<endl;
+ (*myPR2.pr2_kin.q_IK_guess)(0) = DTOR(-20); // turret
+ (*myPR2.pr2_kin.q_IK_guess)(1) = DTOR(-20); // shoulder pitch
+ (*myPR2.pr2_kin.q_IK_guess)(2) = DTOR(40); // shoulder roll
+ (*myPR2.pr2_kin.q_IK_guess)(3) = DTOR(60); // elbow pitch
+ (*myPR2.pr2_kin.q_IK_guess)(4) = DTOR(20); // elbow roll
+ (*myPR2.pr2_kin.q_IK_guess)(5) = DTOR(30); // wrist pitch
+ (*myPR2.pr2_kin.q_IK_guess)(6) = 0.0; // wrist roll
- q_init(0) = DTOR(-20); // turret
- q_init(1) = DTOR(-20); // shoulder pitch
- q_init(2) = DTOR(40); // shoulder roll
- q_init(3) = DTOR(60); // elbow pitch
- q_init(4) = DTOR(20); // elbow roll
- q_init(5) = DTOR(30); // wrist pitch
- q_init(6) = 0.0; // wrist roll
-
- JntArray q_out = JntArray(myPR2.pr2_kin.nJnts);
- myPR2.SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f,q_init,q_out);
- return q_out;
-
-/*
- end effector frame: [[ +0.592488, +0.135547, +0.794094;
- +0.741583, +0.293264, -0.603367;
- -0.314663, +0.946374, +0.0732355]
- [ +0.698411, -0.130820, +0.0626092]]
-*/
-
-// double jntArr[7] = {q_init(0),q_init(1),q_init(2),q_init(3),q_init(4),q_init(5),q_init(6)};
-// double jntSpd[7] = {0,0,0,0,0,0,0};
-// myPR2.SetArmJointPosition(PR2::PR2_RIGHT_ARM, jntArr,jntSpd);
-
+ myPR2.SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f);
}
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pr2_kin_kdl.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pr2_kin_kdl.cpp 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pr2_kin_kdl.cpp 2008-07-28 23:38:31 UTC (rev 2214)
@@ -56,11 +56,10 @@
cout<<"Could not compute Fwd Kin."<<endl;
// send command to robot
- JntArray q_init = JntArray(myPR2.pr2_kin.nJnts);
- JntArray q_out = JntArray(myPR2.pr2_kin.nJnts);
- q_init(0) = 0.0, q_init(1) = 0.0, q_init(2) = 0.0, q_init(3) = 0.0;
- q_init(4) = 0.0, q_init(5) = 0.0, q_init(6) = 0.0;
- myPR2.SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f,q_init,q_out);
+ (*myPR2.pr2_kin.q_IK_guess)(0) = 0.0, (*myPR2.pr2_kin.q_IK_guess)(1) = 0.0, (*myPR2.pr2_kin.q_IK_guess)(2) = 0.0;
+ (*myPR2.pr2_kin.q_IK_guess)(3) = 0.0, (*myPR2.pr2_kin.q_IK_guess)(4) = 0.0, (*myPR2.pr2_kin.q_IK_guess)(5) = 0.0;
+ (*myPR2.pr2_kin.q_IK_guess)(6) = 0.0;
+ myPR2.SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f);
myPR2.GetBasePositionActual(&x, &y, &z, &roll, &pitch, &yaw);
Deleted: pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_kin_controllers.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_kin_controllers.cpp 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_kin_controllers.cpp 2008-07-28 23:38:31 UTC (rev 2214)
@@ -1,133 +0,0 @@
-
-#include "kinematic_controllers.h"
-#include <math.h>
-
-using namespace KDL;
-using namespace PR2;
-using namespace std;
-
-
-#include <termios.h>
-#include <string.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <unistd.h>
-
-int kfd = 0;
-struct termios cooked, raw;
-
-kinematic_controllers myKinCon;
-
-void top();
-void object_pose();
-void go_down();
-void close_gripper();
-void open_gripper();
-
-
-void keyboardLoop()
-{
- 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);
-
- puts("Reading from keyboard");
- puts("---------------------------");
- puts("---------------------------");
-
- for(;;)
- {
- // get the next event from the keyboard
- if(read(kfd, &c, 1) < 0)
- {
- perror("read():");
- exit(-1);
- }
-
- switch(c)
- {
- case '1':
- printf("You pressed 1\n");
- open_gripper();
- top();
- break;
- case '2':
- printf("You pressed 2\n");
- object_pose();
- break;
- case '3':
- printf("You pressed 3\n");
- go_down();
- break;
- case '4':
- printf("You pressed 4\n");
- close_gripper();
- break;
- case '5':
- printf("You pressed 4\n");
- object_pose();
- break;
- case '6':
- printf("You pressed 4\n");
- top();
- break;
- default:
- printf("You pressed Something Else\n");
- break;
- }
- myKinCon.myPR2->hw.UpdateHW();
- }
-}
-
-void top()
-{
- Rotation r = Rotation::RotZ(DTOR(0));
- Vector v(0.3,-0.2,0.56);
- myKinCon.go(v,r,0.05);
-}
-
-void object_pose()
-{
- Rotation r = Rotation::RotZ(DTOR(90));
- Vector v(0.578,0.01,0.06);
- myKinCon.go(v,r,0.05);
-}
-
-void go_down()
-{
- Rotation r = Rotation::RotZ(DTOR(90))*Rotation::RotY(DTOR(30));
- Vector v(0.578,0.01,-0.01);
- myKinCon.go(v,r,0.05);
-}
-
-void close_gripper()
-{
- myKinCon.myPR2->hw.CloseGripper(PR2::PR2_RIGHT_GRIPPER, 0.0, 10000);
-}
-
-void open_gripper()
-{
- myKinCon.myPR2->hw.CloseGripper(PR2::PR2_RIGHT_GRIPPER, 0.15, 500);
-}
-
-void init_robot()
-{
- myKinCon.init();
-}
-
-
-
-int main(int argc, char **argv)
-{
- init_robot();
- keyboardLoop();
- return 0;
-}
-
-
Modified: pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc
===================================================================
--- pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc 2008-07-28 23:38:31 UTC (rev 2214)
@@ -260,10 +260,12 @@
}
if(jointID == ARM_R_GRIPPER) {
this->cmd_rightarmconfig.gripperForceCmd = 50;
- this->cmd_rightarmconfig.gripperGapCmd = .1;
+ this->cmd_rightarmconfig.gripperGapCmd = .2;
+ printf("Opening right gripper\n");
} else {
this->cmd_leftarmconfig.gripperForceCmd = 50;
- this->cmd_leftarmconfig.gripperGapCmd = .1;
+ this->cmd_leftarmconfig.gripperGapCmd = .2;
+ printf("Opening left gripper\n");
}
}
@@ -391,53 +393,52 @@
}
switch(c)
- {
- case 'l':
- case 'L':
- right_arm = false;
- printf("Actuating left arm.\n");
- break;
- case 'r':
- case 'R':
- right_arm = true;
- printf("Actuating right arm.\n");
- break;
- case '+':
- case '=':
- changeJointAngle(curr_jointID, true);
- dirty=true;
- break;
- case '_':
- case '-':
- changeJointAngle(curr_jointID, false);
- dirty=true;
- break;
- case '.':
- _rightInit = false;
- _leftInit = false;
- sleep(1);
- openGripper(curr_jointID);
- dirty = true;
- break;
- case '/':
- _rightInit = false;
- _leftInit = false;
- sleep(1);
- closeGripper(curr_jointID);
- dirty = true;
- break;
- case 'q':
- printCurrentJointValues();
- break;
- case 'k':
- printCurrentEndEffectorWorldCoord();
- break;
- case 'j':
- printCurrentEndEffectorShoulderCoord();
- break;
- default:
- break;
- }
+ {
+ case 'l':
+ case 'L':
+ right_arm = false;
+ printf("Actuating left arm.\n");
+ break;
+ case 'r':
+ case 'R':
+ right_arm = true;
+ printf("Actuating right arm.\n");
+ break;
+ case '+':
+ case '=':
+ changeJointAngle(curr_jointID, true);
+ dirty=true;
+ break;
+ case '_':
+ case '-':
+ changeJointAngle(curr_jointID, false);
+ dirty=true;
+ break;
+ case '.':
+ _rightInit = false;
+ _leftInit = false;
+ openGripper(curr_jointID);
+ dirty = true;
+ break;
+ case '/':
+ _rightInit = false;
+ _leftInit = false;
+ sleep(1);
+ closeGripper(curr_jointID);
+ dirty = true;
+ break;
+ case 'q':
+ printCurrentJointValues();
+ break;
+ case 'k':
+ printCurrentEndEffectorWorldCoord();
+ break;
+ case 'j':
+ printCurrentEndEffectorShoulderCoord();
+ break;
+ default:
+ break;
+ }
if (right_arm==false)
{
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model 2008-07-28 23:38:31 UTC (rev 2214)
@@ -432,26 +432,9 @@
<material>Gazebo/PioneerBody</material>
</visual>
</geom:cylinder>
- <sensor:camera name="wrist_cam_right_sensor">
- <imageSize>640 480</imageSize>
- <hfov>60</hfov>
- <nearClip>0.1</nearClip>
- <farClip>100</farClip>
- <updateRate>15.0</updateRate>
- <!--
- <controller:ros_camera name="wrist_cam_right_controller" plugin="libRos_Camera.so">
- <updateRate>15.0</updateRate>
- <topicName>image_wrist_cam_right</topicName>
- <interface:camera name="wrist_cam_right_iface" />
- </controller:ros_camera>
- -->
- <controller:generic_camera name="wrist_cam_right_controller">
- <updateRate>15.0</updateRate>
- <interface:camera name="wrist_cam_right_iface" />
- </controller:generic_camera>
- </sensor:camera>
</body:cylinder>
+
<body:box name="palm_right">
<xyz> 1.10 -0.15 0.8269</xyz>
<rpy> 0.0 0.0 0.0 </rpy>
@@ -482,6 +465,43 @@
</geom:box>
</body:box>
+ <body:empty name="palm_cam_right_body">
+ <xyz> 1.10 -0.15 0.8569</xyz>
+ <rpy> 0.0 0.0 0.0 </rpy>
+ <sensor:camera name="palm_cam_right_sensor">
+ <imageSize>640 480</imageSize>
+ <hfov>60</hfov>
+ <nearClip>0.1</nearClip>
+ <farClip>100</farClip>
+ <updateRate>15.0</updateRate>
+ <controller:generic_camera name="wrist_cam_right_controller">
+ <updateRate>15.0</updateRate>
+ <interface:camera name="wrist_cam_right_iface" />
+ </controller:generic_camera>
+ </sensor:camera>
+ <geom:box name="palm_cam_right_geom">
+ <xyz> 0.00 0.00 0.00</xyz>
+ <size>0.02 0.02 0.02</size>
+ <mass>0.1</mass>
+ <visual>
+ <size>0.02 0.02 0.02</size>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
+ </geom:box>
+ </body:empty>
+ <joint:hinge name="palm_cam_right">
+ <body1>palm_cam_right_body</body1>
+ <body2>palm_right</body2>
+ <anchor>palm_right</anchor>
+ <axis> 0.0 0.0 1.0 </axis>
+ <lowStop> 0.0 </lowStop>
+ <highStop> 0.0 </highStop>
+ </joint:hinge>
+
+
+
<body:box name="finger_1_right">
<xyz> 1.18 -0.17 0.8269 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world 2008-07-28 23:38:31 UTC (rev 2214)
@@ -81,18 +81,18 @@
<!-- The "desk"-->
<model:physical name="desk1_model">
- <xyz> 2.28 -0.21 -0.10</xyz>
+ <xyz> 0.80 -0.21 -0.10</xyz>
<rpy> 0.0 0.0 0.00</rpy>
<body:box name="desk1_legs_body">
<geom:box name="desk1_legs_geom">
<kp>100000000.0</kp>
<kd>1.0</kd>
- <xyz> 0.0 0.0 0.50</xyz>
+ <xyz> 0.0 0.0 0.60</xyz>
<mesh>default</mesh>
- <size>0.5 1.0 0.75</size>
+ <size>0.5 1.0 0.6</size>
<mass> 10.0</mass>
<visual>
- <size> 0.5 1.0 0.75</size>
+ <size> 0.5 1.0 0.60</size>
<material>Gazebo/Rocky</material>
<mesh>unit_box</mesh>
</visual>
Modified: pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
===================================================================
--- pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-07-28 23:38:31 UTC (rev 2214)
@@ -153,6 +153,10 @@
void cmd_leftarmcartesianReceived();
void cmd_rightarmcartesianReceived();
+ // 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();
+
// laser range data
float ranges[GZ_LASER_MAX_RANGES];
uint8_t intensities[GZ_LASER_MAX_RANGES];
@@ -175,6 +179,8 @@
pr2_msgs::EndEffectorState cmd_leftarmcartesian;
pr2_msgs::EndEffectorState cmd_rightarmcartesian;
+ std_msgs::Empty empty_msg;
+
//Flags to indicate that a new message has arrived
bool newRightArmPos;
bool newLeftArmPos;
Modified: pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
===================================================================
--- pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-28 23:38:31 UTC (rev 2214)
@@ -117,13 +117,18 @@
f.p.data[i] = cmd_leftarmcartesian.trans[i];
}
- KDL::JntArray q = KDL::JntArray(this->PR2Copy->pr2_kin.nJnts);
- this->PR2Copy->GetArmJointPositionCmd(PR2::PR2_LEFT_ARM, q);
- this->PR2Copy->SetArmCartesianPosition(PR2::PR2_LEFT_ARM,f,q,q);
+// KDL::JntArray q = KDL::JntArray(this->PR2Copy->pr2_kin.nJnts);
+// this->PR2Copy->GetArmJointPositionCmd(PR2::PR2_LEFT_ARM, q);
+ this->PR2Copy->SetArmCartesianPosition(PR2::PR2_LEFT_ARM,f);
this->lock.unlock();
}
+void RosGazeboNode::reset_IK_guess()
+{
+ this->PR2Copy->GetArmJointPositionCmd(PR2::PR2_RIGHT_ARM, *(this->PR2Copy->pr2_kin.q_IK_guess));
+}
+
void RosGazeboNode::cmd_rightarmcartesianReceived() {
this->lock.lock();
@@ -135,9 +140,9 @@
f.p.data[i] = cmd_rightarmcartesian.trans[i];
}
- KDL::JntArray q = KDL::JntArray(this->PR2Copy->pr2_kin.nJnts);
- this->PR2Copy->GetArmJointPositionCmd(PR2::PR2_RIGHT_ARM, q);
- this->PR2Copy->SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f, q,q);
+// KDL::JntArray q = KDL::JntArray(this->PR2Copy->pr2_kin.nJnts);
+// this->PR2Copy->GetArmJointPositionCmd(PR2::PR2_RIGHT_ARM, q);
+ this->PR2Copy->SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,f);
this->lock.unlock();
}
@@ -346,6 +351,7 @@
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);
return(0);
}
Modified: pkg/trunk/util/kinematics/interpolated_kinematic_controller/src/interpolated_kinematic_controller.cc
===================================================================
--- pkg/trunk/util/kinematics/interpolated_kinematic_controller/src/interpolated_kinematic_controller.cc 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/util/kinematics/interpolated_kinematic_controller/src/interpolated_kinematic_controller.cc 2008-07-28 23:38:31 UTC (rev 2214)
@@ -1,6 +1,7 @@
#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>
@@ -22,6 +23,8 @@
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, &InterpolatedKinematicController::setRightEndEffector);
subscribe("left_pr2arm_set_end_effector", _leftEndEffectorGoal, &InterpolatedKinematicController::setLeftEndEffector);
subscribe("left_pr2arm_pos", leftArmPosMsg, &InterpolatedKinematicController::currentLeftArmPos);
@@ -96,6 +99,9 @@
void RunControlLoop(bool isRightArm, const Frame& r, const std_msgs::PR2Arm& arm) {
+ std_msgs::Empty emp;
+ publish("reset_IK_guess",emp);
+
PR2_kinematics pr2_kin;
JntArray q = JntArray(pr2_kin.nJnts);
Modified: pkg/trunk/util/kinematics/libKDL/CMakeLists.txt
===================================================================
--- pkg/trunk/util/kinematics/libKDL/CMakeLists.txt 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/util/kinematics/libKDL/CMakeLists.txt 2008-07-28 23:38:31 UTC (rev 2214)
@@ -11,6 +11,6 @@
rospack_add_executable(bin/pr2_kin_test test/pr2_kin_test.cpp)
target_link_libraries(bin/pr2_kin_test KDL orocos-kdl)
-#rospack_add_executable(bin/dynamics test/dynamics.cpp)
-#target_link_libraries(bin/dynamics KDL orocos-kdl)
+rospack_add_executable(bin/dynamics test/dynamics.cpp)
+target_link_libraries(bin/dynamics KDL orocos-kdl)
Modified: pkg/trunk/util/kinematics/libKDL/include/libKDL/kdl_kinematics.h
===================================================================
--- pkg/trunk/util/kinematics/libKDL/include/libKDL/kdl_kinematics.h 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/util/kinematics/libKDL/include/libKDL/kdl_kinematics.h 2008-07-28 23:38:31 UTC (rev 2214)
@@ -35,6 +35,9 @@
KDL::ChainIkSolverPos_NR *ik_pos_solver;
int nJnts;
+ KDL::JntArray *q_IK_guess; // is used as the IK guess.
+ KDL::JntArray *q_IK_result; // stores result of IK
+
KDL::Chain *chain_forearm_camera; // chain whose end-effector is the fore-arm camera.
KDL::ChainFkSolverPos_recursive *fk_pos_solver_forearm_camera;
KDL::ChainIkSolverVel_pinv *ik_vel_solver_forearm_camera;
@@ -46,6 +49,7 @@
~PR2_kinematics();
bool FK(const KDL::JntArray &q, KDL::Frame &f);
bool IK(const KDL::JntArray &q_init, const KDL::Frame &f, KDL::JntArray &q_out);
+ bool IK(const KDL::Frame &f);
};
Modified: pkg/trunk/util/kinematics/libKDL/src/kdl_kinematics.cpp
===================================================================
--- pkg/trunk/util/kinematics/libKDL/src/kdl_kinematics.cpp 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/util/kinematics/libKDL/src/kdl_kinematics.cpp 2008-07-28 23:38:31 UTC (rev 2214)
@@ -19,6 +19,9 @@
this->chain->addSegment(Segment(Joint(Joint::RotX),Frame(Vector(WRIST_ROLL_GRIPPER_OFFSET.x,0.0,0.0))));
this->nJnts = this->chain->getNrOfJoints();
+ this->q_IK_guess = new JntArray(this->nJnts);
+ this->q_IK_result = new JntArray(this->nJnts);
+
//------ Forward Position Kinematics --------------
this->fk_pos_solver = new ChainFkSolverPos_recursive(*this->chain);
//------- IK
@@ -51,6 +54,8 @@
delete this->fk_pos_solver;
delete this->ik_vel_solver;
delete this->ik_pos_solver;
+ delete this->q_IK_guess;
+ delete this->q_IK_result;
delete this->chain_forearm_camera;
delete this->fk_pos_solver_forearm_camera;
@@ -71,6 +76,10 @@
return false;
}
+/*
+ * We might want to get rid of this function.
+ * Hasn't been removed for compatibility with ArmController.
+ */
bool PR2_kinematics::IK(const JntArray &q_init, const Frame &f, JntArray &q_out)
{
if (this->ik_pos_solver->CartToJnt(q_init,f,q_out) >= 0)
@@ -82,7 +91,25 @@
return false;
}
+/*
+ * uses internal state (q_IK_guess) as the seed for KDL's IK.
+ * result is stored in q_IK_result
+ */
+bool PR2_kinematics::IK(const Frame &f)
+{
+ if (this->ik_pos_solver->CartToJnt(*this->q_IK_guess,f,*this->q_IK_result) >= 0)
+ {
+ angle_within_mod180(*this->q_IK_result, this->nJnts);
+ cout<<"IK guess:"<<*this->q_IK_guess<<endl;
+ cout<<"IK result:"<<*this->q_IK_result<<endl;
+ return true;
+ }
+ else
+ return false;
+}
+
+
// returns a%b
double modulus_double(double a, double b)
{
Modified: pkg/trunk/util/kinematics/libKDL/test/pr2_kin_test.cpp
===================================================================
--- pkg/trunk/util/kinematics/libKDL/test/pr2_kin_test.cpp 2008-07-28 23:37:45 UTC (rev 2213)
+++ pkg/trunk/util/kinematics/libKDL/test/pr2_kin_test.cpp 2008-07-28 23:38:31 UTC (rev 2214)
@@ -27,16 +27,14 @@
else
cout<<"Could not compute Fwd Kin."<<endl;
- JntArray q_init = JntArray(pr2_kin.nJnts);
- q_init(0) = 0.1, q_init(1) = 0.0, q_init(2) = 0.0, q_init(3) = 0.0;
- q_init(4) = 0.0, q_init(5) = 0.0, q_init(6) = 0.0;
+ (*pr2_kin.q_IK_guess)(0) = 0.1, (*pr2_kin.q_IK_guess)(1) = 0.0, (*pr2_kin.q_IK_guess)(2) = 0.0, (*pr2_kin.q_IK_guess)(3) = 0.0;
+ (*pr2_kin.q_IK_guess)(4) = 0.0, (*pr2_kin.q_IK_guess)(5) = 0.0, (*pr2_kin.q_IK_guess)(6) = 0.0;
- JntArray q_out = JntArray(pr2_kin.nJnts);
gettimeofday(&t0,NULL);
- if (pr2_kin.IK(q_init, f, q_out) == true)
+ if (pr2_kin.IK(f) == true)
{
gettimeofday(&t1, NULL);
- cout<<"IK result:"<<q_out<<endl;
+ cout<<"IK result:"<<*pr2_kin.q_IK_result<<endl;
double time_taken = (t1.tv_sec*1000000+t1.tv_usec - (t0.tv_sec*1000000+t0.tv_usec))/1000.;
printf("Time taken: %f ms\n", time_taken);
}
@@ -45,7 +43,7 @@
//------ checking that IK returned a valid soln -----
Frame f_ik;
- if (pr2_kin.FK(q_out,f_ik))
+ if (pr2_kin.FK(*pr2_kin.q_IK_result,f_ik))
cout<<"End effector after IK:"<<f_ik<<endl;
else
cout<<"Could not compute Fwd Kin. (After IK)"<<endl;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
