Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [687] pkg/trunk
|
From: <sac...@us...> - 2008-06-06 21:10:10
|
Revision: 687
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=687&view=rev
Author: sachinchitta
Date: 2008-06-06 14:10:09 -0700 (Fri, 06 Jun 2008)
Log Message:
-----------
Fixed nan problem with test_pr2API, Cartesian control (IK-based) now works (see test_pr2AP
I.cpp for an example of how to use it)
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2API/Makefile
pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_pr2API.cpp
pkg/trunk/util/kinematics/libKinematics/Makefile
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/Makefile
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/Makefile 2008-06-06 18:31:23 UTC (rev 686)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/Makefile 2008-06-06 21:10:09 UTC (rev 687)
@@ -1,11 +1,12 @@
PKG = libpr2API
CXX = g++
-CFLAGS = -g -Wall $(shell rospack export/cpp/cflags $(PKG))
+CFLAGS = -ggdb -g -Wall $(shell rospack export/cpp/cflags $(PKG))
LDFLAGS = $(shell rospack export/cpp/lflags gazebo) \
$(shell rospack export/cpp/lflags pr2Core) \
- $(shell rospack export/cpp/lflags libKinematics)
+ $(shell rospack export/cpp/lflags libKinematics) \
+ $(shell rospack export/cpp/lflags newmat10)
LFLAGS = $(shell rospack export/cpp/lflags $(PKG))
@@ -23,11 +24,8 @@
$(LIBS): src/pr2API.cpp
$(CXX) $(CFLAGS) -shared -o $@ $^ $(LDFLAGS)
-test_pr2API.o: src/test/test_pr2API.cpp
- $(CXX) $(CFLAGS) -o $@ $< $(LFLAGS)
-
test_libpr2API: src/test/test_pr2API.cpp
- $(CXX) $(CFLAGS) -o $@ $< $(LFLAGS)
+ $(CXX) $(CFLAGS) -o $@ $< $(LFLAGS) $(LDFLAGS)
clean:
rm -f test_pr2API $(LIBP) $(LIBS)
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml 2008-06-06 18:31:23 UTC (rev 686)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml 2008-06-06 21:10:09 UTC (rev 687)
@@ -7,10 +7,10 @@
<author>Sachin Chitta</author>
<license>BSD</license>
<url>http://pr.willowgarage.com</url>
+<depend package="newmat10"/>
<depend package="gazebo"/>
<depend package="pr2Core"/>
<depend package="libKinematics"/>
-<depend package="newmat10"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lpr2API"/>
</export>
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-06 18:31:23 UTC (rev 686)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-06 21:10:09 UTC (rev 687)
@@ -564,8 +564,7 @@
g(3,4) = g(3,4) - SPINE_LEFT_ARM_OFFSET.z;
}
theta = 0;
- theta = myArm.ComputeIK(g,0);
-
+ theta = myArm.ComputeIK(g,0.1);
for(int jj = 1; jj <= 8; jj++)
{
if (theta(8,jj) > -1)
@@ -574,15 +573,14 @@
break;
}
}
-
if(validSolution <= 8)
{
for(int ii = 0; ii < 7; ii++)
{
angles[ii] = theta(ii+1,validSolution);
speeds[ii] = 0;
+ this->SetJointServoCmd((PR2::PR2_JOINT_ID) (JointStart[id]+ii),angles[ii],0);
}
- SetArmJointPosition(id,angles,speeds);
}
return PR2_ALL_OK;
};
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_pr2API.cpp 2008-06-06 18:31:23 UTC (rev 686)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_pr2API.cpp 2008-06-06 21:10:09 UTC (rev 687)
@@ -3,7 +3,7 @@
#include <pr2Core/pr2Core.h>
#include <math.h>
-//using namespace kinematics;
+using namespace kinematics;
using namespace PR2;
using namespace std;
@@ -19,16 +19,13 @@
myPR2.SetBaseControlMode(PR2_CARTESIAN_CONTROL);
myPR2.SetBaseCartesianSpeedCmd(0.0,-0.5,1*M_PI/8);
-
-
-
// create a end-effector position and orientation for inverse kinematics test
NEWMAT::Matrix g(4,4);
g = 0;
PR2Arm myArm;
- /* Joint angles (radians) and speeds for testing */
+ // Joint angles (radians) and speeds for testing
double angles[7] = {0,0,0,0,0,0,0};
angles[0] = 0.1; // shoulder pan angle
@@ -39,25 +36,27 @@
angles[5] = 0.5; // wrist pitch angle
angles[6] = 0.0; // wrist roll
- NEWMAT::Matrix pose = myArm.ComputeFK(angles);
+ NEWMAT::Matrix pose = myArm.ComputeFK(angles);
g = pose;
// offset from the base of the arm,
// subtracted for PR2 to cancel out effect of offset
// watchout whether left of right arm is used
- g(1,4) = g(1,4) + PR2::SPINE_RIGHT_ARM_OFFSET.x;
- g(2,4) = g(2,4) + PR2::SPINE_RIGHT_ARM_OFFSET.y;
- g(3,4) = g(3,4) + PR2::SPINE_RIGHT_ARM_OFFSET.z;
- cout << "Main::End-effector Pose:" << endl << g << endl ;
+
- // send command to robot
- myPR2.SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,g);
-
- // compute analytical solution
+ // compute analytical solution
NEWMAT::Matrix theta(8,8);
theta = 0;
theta = myArm.ComputeIK(g,0.1);
PrintMatrix(theta,"exact solution for pos/orien of end effector");
+ cout << "Main::End-effector Pose:" << endl << g << endl ;
+ g(1,4) = g(1,4) + PR2::SPINE_RIGHT_ARM_OFFSET.x;
+ g(2,4) = g(2,4) + PR2::SPINE_RIGHT_ARM_OFFSET.y;
+ g(3,4) = g(3,4) + PR2::SPINE_RIGHT_ARM_OFFSET.z;
+ // send command to robot
+ myPR2.SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,g);
+
+
return 0;
};
Modified: pkg/trunk/util/kinematics/libKinematics/Makefile
===================================================================
--- pkg/trunk/util/kinematics/libKinematics/Makefile 2008-06-06 18:31:23 UTC (rev 686)
+++ pkg/trunk/util/kinematics/libKinematics/Makefile 2008-06-06 21:10:09 UTC (rev 687)
@@ -23,7 +23,7 @@
ar -rs $@ $^
test_PR2ik: src/test/test_PR2ik.cpp
- g++ $(CFLAGS) -o $@ $^ $(LDFLAGS)
+ g++ $(CFLAGS) -o $@ $^ -lnewmat
clean:
rm -rf test_PR2ik
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
