Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2838] pkg/trunk
|
From: <adv...@us...> - 2008-08-08 22:52:02
|
Revision: 2838
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2838&view=rev
Author: advaitjain
Date: 2008-08-08 22:52:09 +0000 (Fri, 08 Aug 2008)
Log Message:
-----------
moved libKDL to deprecated
Added Paths:
-----------
pkg/trunk/deprecated/libKDL/
pkg/trunk/deprecated/libKDL/src/kdl_kinematics.cpp
pkg/trunk/deprecated/libKDL/test/pr2_kin_test.cpp
Removed Paths:
-------------
pkg/trunk/deprecated/libKDL/src/kdl_kinematics.cpp
pkg/trunk/deprecated/libKDL/test/pr2_kin_test.cpp
pkg/trunk/util/kinematics/libKDL/
Deleted: pkg/trunk/deprecated/libKDL/src/kdl_kinematics.cpp
===================================================================
--- pkg/trunk/util/kinematics/libKDL/src/kdl_kinematics.cpp 2008-08-08 22:04:51 UTC (rev 2829)
+++ pkg/trunk/deprecated/libKDL/src/kdl_kinematics.cpp 2008-08-08 22:52:09 UTC (rev 2838)
@@ -1,140 +0,0 @@
-
-#include "libKDL/kdl_kinematics.h"
-
-using namespace KDL;
-using namespace PR2;
-using namespace std;
-
-
-PR2_kinematics::PR2_kinematics()
-{
- //---------- create the chain for the PR2 ----------
- this->chain = new Chain();
- this->chain->addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(ARM_PAN_SHOULDER_PITCH_OFFSET.x,0.0,0.0))));
- this->chain->addSegment(Segment(Joint(Joint::RotY),Frame(Vector(ARM_SHOULDER_PITCH_ROLL_OFFSET.x,0.0,0.0))));
- this->chain->addSegment(Segment(Joint(Joint::RotX),Frame(Vector(ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.x,0.0,0.0))));
- this->chain->addSegment(Segment(Joint(Joint::RotY),Frame(Vector(ELBOW_PITCH_ELBOW_ROLL_OFFSET.x,0.0,0.0))));
- this->chain->addSegment(Segment(Joint(Joint::RotX),Frame(Vector(ELBOW_ROLL_WRIST_PITCH_OFFSET.x,0.0,0.0))));
- this->chain->addSegment(Segment(Joint(Joint::RotY),Frame(Vector(WRIST_PITCH_WRIST_ROLL_OFFSET.x,0.0,0.0))));
- 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
- this->ik_vel_solver = new ChainIkSolverVel_pinv(*this->chain);
- this->ik_pos_solver = new ChainIkSolverPos_NR(*this->chain, *this->fk_pos_solver, *this->ik_vel_solver);
-
-
- //---------- create a chain for the fore-arm camera -------------
- this->chain_forearm_camera = new Chain();
- this->chain_forearm_camera->addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(ARM_PAN_SHOULDER_PITCH_OFFSET.x,0.0,0.0))));
- this->chain_forearm_camera->addSegment(Segment(Joint(Joint::RotY),Frame(Vector(ARM_SHOULDER_PITCH_ROLL_OFFSET.x,0.0,0.0))));
- this->chain_forearm_camera->addSegment(Segment(Joint(Joint::RotX),Frame(Vector(ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.x,0.0,0.0))));
- this->chain_forearm_camera->addSegment(Segment(Joint(Joint::RotY),Frame(Vector(ELBOW_PITCH_ELBOW_ROLL_OFFSET.x,0.0,0.0))));
- this->chain_forearm_camera->addSegment(Segment(Joint(Joint::RotX),Frame(Rotation::RotY(-45*deg2rad),Vector(0.111825,0.0,0.02))));
-
- this->nJnts_forearm_camera = this->chain_forearm_camera->getNrOfJoints();
- //------ Forward Position Kinematics --------------
- this->fk_pos_solver_forearm_camera = new ChainFkSolverPos_recursive(*this->chain_forearm_camera);
- //------- IK
- this->ik_vel_solver_forearm_camera = new ChainIkSolverVel_pinv(*this->chain_forearm_camera);
- this->ik_pos_solver_forearm_camera = new ChainIkSolverPos_NR(*this->chain_forearm_camera, *this->fk_pos_solver_forearm_camera,
- *this->ik_vel_solver_forearm_camera);
-
-
-}
-
-PR2_kinematics::~PR2_kinematics()
-{
- delete this->chain;
- 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;
- delete this->ik_vel_solver_forearm_camera;
- delete this->ik_pos_solver_forearm_camera;
-}
-
-
-/*
- * f - end effector frame (result of the fwd kinematics)
- * returns True if ok, False if error.
- */
-bool PR2_kinematics::FK(const JntArray &q, Frame &f)
-{
- if (this->fk_pos_solver->JntToCart(q,f) >= 0)
- return true;
- else
- 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)
- {
- angle_within_mod180(q_out, this->nJnts);
- return true;
- }
- else
- 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);
- return true;
- }
- else
- return false;
-}
-
-
-// returns a%b
-double modulus_double(double a, double b)
-{
- int quo = a/b;
- return a-b*quo;
-}
-
-double angle_within_mod180(double ang)
-{
- double rem = modulus_double(ang, 2*M_PI);
-
- if (rem>M_PI)
- rem -= 2*M_PI;
- else if (rem<(-1*M_PI))
- rem += 2*M_PI;
-
- return rem;
-}
-
-void angle_within_mod180(JntArray &q, int nJnts)
-{
- for(int i=0;i<nJnts;i++)
- {
- q(i) = angle_within_mod180(q(i));
-// printf(".. %f ..", q(i));
- }
-}
-
-
-
-
Copied: pkg/trunk/deprecated/libKDL/src/kdl_kinematics.cpp (from rev 2837, pkg/trunk/util/kinematics/libKDL/src/kdl_kinematics.cpp)
===================================================================
--- pkg/trunk/deprecated/libKDL/src/kdl_kinematics.cpp (rev 0)
+++ pkg/trunk/deprecated/libKDL/src/kdl_kinematics.cpp 2008-08-08 22:52:09 UTC (rev 2838)
@@ -0,0 +1,144 @@
+
+#include "libKDL/kdl_kinematics.h"
+
+using namespace KDL;
+using namespace PR2;
+using namespace std;
+
+
+PR2_kinematics::PR2_kinematics()
+{
+ //---------- create the chain for the PR2 ----------
+ this->chain = new Chain();
+ this->chain->addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(ARM_PAN_SHOULDER_PITCH_OFFSET.x,0.0,0.0))));
+ this->chain->addSegment(Segment(Joint(Joint::RotY),Frame(Vector(ARM_SHOULDER_PITCH_ROLL_OFFSET.x,0.0,0.0))));
+ this->chain->addSegment(Segment(Joint(Joint::RotX),Frame(Vector(ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.x,0.0,0.0))));
+ this->chain->addSegment(Segment(Joint(Joint::RotY),Frame(Vector(ELBOW_PITCH_ELBOW_ROLL_OFFSET.x,0.0,0.0))));
+ this->chain->addSegment(Segment(Joint(Joint::RotX),Frame(Vector(ELBOW_ROLL_WRIST_PITCH_OFFSET.x,0.0,0.0))));
+ this->chain->addSegment(Segment(Joint(Joint::RotY),Frame(Vector(WRIST_PITCH_WRIST_ROLL_OFFSET.x,0.0,0.0))));
+ 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
+ this->ik_vel_solver = new ChainIkSolverVel_pinv(*this->chain);
+ this->ik_pos_solver = new ChainIkSolverPos_NR(*this->chain, *this->fk_pos_solver, *this->ik_vel_solver);
+
+
+ //---------- create a chain for the fore-arm camera -------------
+ this->chain_forearm_camera = new Chain();
+ this->chain_forearm_camera->addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(ARM_PAN_SHOULDER_PITCH_OFFSET.x,0.0,0.0))));
+ this->chain_forearm_camera->addSegment(Segment(Joint(Joint::RotY),Frame(Vector(ARM_SHOULDER_PITCH_ROLL_OFFSET.x,0.0,0.0))));
+ this->chain_forearm_camera->addSegment(Segment(Joint(Joint::RotX),Frame(Vector(ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.x,0.0,0.0))));
+ this->chain_forearm_camera->addSegment(Segment(Joint(Joint::RotY),Frame(Vector(ELBOW_PITCH_ELBOW_ROLL_OFFSET.x,0.0,0.0))));
+ this->chain_forearm_camera->addSegment(Segment(Joint(Joint::RotX),Frame(Rotation::RotY(-45*deg2rad),Vector(0.111825,0.0,0.02))));
+
+ this->nJnts_forearm_camera = this->chain_forearm_camera->getNrOfJoints();
+ //------ Forward Position Kinematics --------------
+ this->fk_pos_solver_forearm_camera = new ChainFkSolverPos_recursive(*this->chain_forearm_camera);
+ //------- IK
+ this->ik_vel_solver_forearm_camera = new ChainIkSolverVel_pinv(*this->chain_forearm_camera);
+ this->ik_pos_solver_forearm_camera = new ChainIkSolverPos_NR(*this->chain_forearm_camera, *this->fk_pos_solver_forearm_camera,
+ *this->ik_vel_solver_forearm_camera);
+
+
+}
+
+PR2_kinematics::~PR2_kinematics()
+{
+ delete this->chain;
+ 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;
+ delete this->ik_vel_solver_forearm_camera;
+ delete this->ik_pos_solver_forearm_camera;
+}
+
+
+/*
+ * f - end effector frame (result of the fwd kinematics)
+ * returns True if ok, False if error.
+ */
+bool PR2_kinematics::FK(const JntArray &q, Frame &f)
+{
+ if (this->fk_pos_solver->JntToCart(q,f) >= 0)
+ return true;
+ else
+ 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)
+ {
+ angle_within_mod180(q_out, this->nJnts);
+ return true;
+ }
+ else
+ {
+ printf("[libKDL]<kdl_kinematics.cpp>IK failed.\n");
+ 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<<"[libKDL]<kdl_kinematics.cpp> IK result: "<<*this->q_IK_result<<endl;
+ return true;
+ }
+ else
+ return false;
+}
+
+
+// returns a%b
+double modulus_double(double a, double b)
+{
+ int quo = a/b;
+ return a-b*quo;
+}
+
+double angle_within_mod180(double ang)
+{
+ double rem = modulus_double(ang, 2*M_PI);
+
+ if (rem>M_PI)
+ rem -= 2*M_PI;
+ else if (rem<(-1*M_PI))
+ rem += 2*M_PI;
+
+ return rem;
+}
+
+void angle_within_mod180(JntArray &q, int nJnts)
+{
+ for(int i=0;i<nJnts;i++)
+ {
+ q(i) = angle_within_mod180(q(i));
+// printf(".. %f ..", q(i));
+ }
+}
+
+
+
+
Deleted: pkg/trunk/deprecated/libKDL/test/pr2_kin_test.cpp
===================================================================
--- pkg/trunk/util/kinematics/libKDL/test/pr2_kin_test.cpp 2008-08-08 22:04:51 UTC (rev 2829)
+++ pkg/trunk/deprecated/libKDL/test/pr2_kin_test.cpp 2008-08-08 22:52:09 UTC (rev 2838)
@@ -1,61 +0,0 @@
-
-#include "libKDL/kdl_kinematics.h"
-
-#include <sys/time.h>
-#include <unistd.h>
-
-
-using namespace KDL;
-using namespace PR2;
-using namespace std;
-
-
-
-int main( int argc, char** argv )
-{
- PR2_kinematics pr2_kin;
- struct timeval t0, t1;
-
- JntArray pr2_config = JntArray(pr2_kin.nJnts);
- pr2_config(0) = 0.0, pr2_config(1) = -0, pr2_config(2)=0.0, pr2_config(3)=0.0;
- pr2_config(4) = 0.0, pr2_config(5) = deg2rad*0, pr2_config(6) = deg2rad*90.0;
- cout<<"Config of the arm:"<<pr2_config<<endl;
-
- Frame f;
- if (pr2_kin.FK(pr2_config,f))
- cout<<"End effector transformation:"<<f<<endl;
-
- pr2_config(0) = 0.1, pr2_config(1) = -1, pr2_config(2)=0.3, pr2_config(3)=0.3;
- pr2_config(4) = 0.2, pr2_config(5) = 0.5, pr2_config(6) = 0.0;
- cout<<"Config of the arm:"<<pr2_config<<endl;
-
- if (pr2_kin.FK(pr2_config,f))
- cout<<"End effector transformation:"<<f<<endl;
- else
- cout<<"Could not compute Fwd Kin."<<endl;
-
- (*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;
-
- gettimeofday(&t0,NULL);
- if (pr2_kin.IK(f) == true)
- {
- gettimeofday(&t1, NULL);
- 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);
- }
- else
- cout<<"Could not compute Inv Kin."<<endl;
-
- //------ checking that IK returned a valid soln -----
- Frame 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;
-
-}
-
-
-
Copied: pkg/trunk/deprecated/libKDL/test/pr2_kin_test.cpp (from rev 2837, pkg/trunk/util/kinematics/libKDL/test/pr2_kin_test.cpp)
===================================================================
--- pkg/trunk/deprecated/libKDL/test/pr2_kin_test.cpp (rev 0)
+++ pkg/trunk/deprecated/libKDL/test/pr2_kin_test.cpp 2008-08-08 22:52:09 UTC (rev 2838)
@@ -0,0 +1,69 @@
+
+#include "libKDL/kdl_kinematics.h"
+
+#include <sys/time.h>
+#include <unistd.h>
+
+
+using namespace KDL;
+using namespace PR2;
+using namespace std;
+
+
+
+int main( int argc, char** argv )
+{
+ PR2_kinematics pr2_kin;
+ struct timeval t0, t1;
+
+ JntArray pr2_config = JntArray(pr2_kin.nJnts);
+ pr2_config(0) = 0.0, pr2_config(1) = -0, pr2_config(2)=0.0, pr2_config(3)=0.0;
+ pr2_config(4) = 0.0, pr2_config(5) = deg2rad*0, pr2_config(6) = deg2rad*0.0;
+ cout<<"Config of the arm:"<<pr2_config<<endl;
+
+// pr2_config(0) = -0.641904;
+// pr2_config(1) = -0.496190;
+// pr2_config(2) = +0.201855;
+// pr2_config(3) = +0.845452;
+// pr2_config(4) = -0.187794;
+// pr2_config(5) = +1.23572;
+// pr2_config(6) = -0.482375;
+
+ Frame f;
+ if (pr2_kin.FK(pr2_config,f))
+ cout<<"End effector transformation:"<<f<<endl;
+
+// pr2_config(0) = 0.1, pr2_config(1) = -1, pr2_config(2)=0.3, pr2_config(3)=0.3;
+// pr2_config(4) = 0.2, pr2_config(5) = 0.5, pr2_config(6) = 0.0;
+// cout<<"Config of the arm:"<<pr2_config<<endl;
+//
+// if (pr2_kin.FK(pr2_config,f))
+// cout<<"End effector transformation:"<<f<<endl;
+// else
+// cout<<"Could not compute Fwd Kin."<<endl;
+//
+// (*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;
+//
+// gettimeofday(&t0,NULL);
+// if (pr2_kin.IK(f) == true)
+// {
+// gettimeofday(&t1, NULL);
+// 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);
+// }
+// else
+// cout<<"Could not compute Inv Kin."<<endl;
+//
+// //------ checking that IK returned a valid soln -----
+// Frame 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.
|
