Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [743] pkg/trunk
|
From: <sac...@us...> - 2008-06-11 17:49:49
|
Revision: 743
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=743&view=rev
Author: sachinchitta
Date: 2008-06-11 10:49:55 -0700 (Wed, 11 Jun 2008)
Log Message:
-----------
) Fixed inverted axes in Gazebo
(b) Added test code for inverse kinematics, IK now works with a parameterization of the first angle. TODO: further testing across configuration space, addition of two other IK methods, better tuning of low-level controllers
(c) Added test code for individual joint tests
(d) Added base "steering" cmd to steer the wheels in the right direction before motion is initiated
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2API/Makefile
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/test_pr2API.cpp
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
pkg/trunk/simulators/gazebo_plugin/Makefile
pkg/trunk/simulators/gazebo_plugin/SConstruct
pkg/trunk/simulators/gazebo_plugin/manifest.xml
pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc
pkg/trunk/simulators/rosgazebo/world/pr2.model
pkg/trunk/simulators/rosgazebo/world/robot.world
pkg/trunk/util/kinematics/libKinematics/include/libKinematics/kinematics.h
pkg/trunk/util/kinematics/libKinematics/src/ik.cpp
pkg/trunk/util/kinematics/libKinematics/src/kinematics.cpp
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/Makefile
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/Makefile 2008-06-11 17:24:12 UTC (rev 742)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/Makefile 2008-06-11 17:49:55 UTC (rev 743)
@@ -1,7 +1,7 @@
PKG = libpr2API
CXX = g++
-CFLAGS = -ggdb -g -Wall $(shell rospack export/cpp/cflags $(PKG))
+CFLAGS = -g -Wall $(shell rospack export/cpp/cflags $(PKG))
CDFLAGS = $(shell rospack export/cpp/cflags gazebo) \
$(shell rospack export/cpp/cflags pr2Core) \
@@ -30,7 +30,7 @@
$(LIBS): pr2API.o
$(CXX) $(CDFLAGS) -shared -o $@ $^ $(LDFLAGS)
-test_libpr2API: src/test/test_pr2API.cpp
+test_libpr2API: src/test/test_pr2API.cpp pr2API.o
$(CXX) $(CFLAGS) -o $@ $< $(LFLAGS)
clean:
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-11 17:24:12 UTC (rev 742)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-11 17:49:55 UTC (rev 743)
@@ -30,8 +30,8 @@
//ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
//POSSIBILITY OF SUCH DAMAGE.
-#ifndef PR2_API_HH
-#define PR2_API_HH
+#ifndef PR2NEW_API_HH
+#define PR2NEW_API_HH
#include <newmat10/newmat.h>
#include <libKinematics/ik.h>
#include <pr2Core/pr2Core.h>
@@ -47,6 +47,7 @@
class PR2Robot
{
+
/*! \fn
\brief Constructor
*/
@@ -73,7 +74,7 @@
/*! \fn
\brief Set the joint control mode
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs
\param mode - mode for joint control, possible options are torque control, position control or speed control
*/
public: PR2_ERROR_CODE SetJointControlMode(PR2_JOINT_ID id, PR2_JOINT_CONTROL_MODE mode);
@@ -81,7 +82,7 @@
/*! \fn
\brief Get the joint control mode
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs
\param mode - pointer to return value (mode for joint control, possible values are torque control, position control or speed control)
*/
public: PR2_ERROR_CODE GetJointControlMode(PR2_JOINT_ID id, PR2_JOINT_CONTROL_MODE *mode);
@@ -89,7 +90,7 @@
/*! \fn
\brief Set the controller gains
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs
\param pGain - proportional gain
\param iGain - integral gain
\param dGain - derivative gain
@@ -99,7 +100,7 @@
/*! \fn
\brief Get the controller gains
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs
\param pGain - pointer to proportional gain
\param iGain - pointer to integral gain
\param dGain - pointer to derivative gain
@@ -109,29 +110,29 @@
/*! \fn
\brief Enable the joint
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs
*/
public: PR2_ERROR_CODE EnableJoint(PR2_JOINT_ID id);
/*! \fn
\brief Disable the joint
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs
*/
public: PR2_ERROR_CODE DisableJoint(PR2_JOINT_ID id);
/*! \fn
\brief Return value corresponding to whether the joint is enabled or not, 0 - disabled, 1 - enabled
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs
*/
public: PR2_ERROR_CODE IsEnabledJoint(PR2_JOINT_ID id, int *enabled);
/*! \fn
\brief Set particular parameters for the joint (NOT IMPLEMENTED YET)
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
- \param pId - parameter ID corresponding to the parameter for the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
+ \param pId - parameter ID corresponding to the parameter for the joint, see pr2Core.h for a list of joint IDs and parameter IDs
\param value - parameter value
*/
public: PR2_ERROR_CODE SetJointParams(PR2_JOINT_ID id, PR2_JOINT_PARAM_ID pId, double value);
@@ -139,15 +140,15 @@
/*! \fn
\brief Get particular parameters for the joint (NOT IMPLEMENTED YET)
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
- \param pId - parameter ID corresponding to the parameter for the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
+ \param pId - parameter ID corresponding to the parameter for the joint, see pr2Core.h for a list of joint IDs and parameter IDs
*/
public: PR2_ERROR_CODE GetJointParams(PR2_JOINT_ID id, PR2_JOINT_PARAM_ID pId, double *value);
/*! \fn
\brief Command a desired joint position and speed
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
\param jointPosition - desired joint position (in radians or meters)
\param jointSpeed - desired joint speed (in rad/s or m/s)
*/
@@ -156,7 +157,7 @@
/*! \fn
\brief Get the commanded joint position and speed values
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
\param jointPosition - pointer to desired joint position (in radians or meters)
\param jointSpeed - desired joint speed (in rad/s or m/s)
*/
@@ -164,7 +165,7 @@
/*! \fn
\brief Get the actual joint position and speed values
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
\param jointPosition - pointer to desired joint position (in radians or meters)
\param jointSpeed - desired joint speed (in rad/s or m/s)
*/
@@ -172,7 +173,7 @@
/*! \fn
\brief Get the actual joint position and speed values
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
\param jointPosition - pointer to desired joint position (in radians or meters)
\param jointSpeed - desired joint speed (in rad/s or m/s)
*/
@@ -180,35 +181,35 @@
/*! \fn
\brief Command a desired joint torque or force (for prismatic joints)
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
\param torque - desired torque (Nm or N)
*/
public: PR2_ERROR_CODE SetJointTorque(PR2_JOINT_ID id, double torque);
/*! \fn
\brief Get the commanded joint torque or force (for prismatic joints)
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
\param torque - pointer to return value for commanded torque (Nm or N)
*/
public: PR2_ERROR_CODE GetJointTorqueCmd(PR2_JOINT_ID id, double *torque);
/*! \fn
\brief Get the actual joint torque or force (for prismatic joints)
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
\param torque - pointer to return value for actual torque (Nm or N)
*/
public: PR2_ERROR_CODE GetJointTorqueActual(PR2_JOINT_ID id, double *torque);
/*! \fn
\brief Command a desired joint speed
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
\param torque - desired speed (rad/s or m/s)
*/
public: PR2_ERROR_CODE SetJointSpeed(PR2_JOINT_ID id, double speed);
/*! \fn
\brief Get the commanded joint speed
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
\param torque - desired speed (rad/s or m/s)
*/
public: PR2_ERROR_CODE GetJointSpeedCmd(PR2_JOINT_ID id, double *speed);
@@ -216,7 +217,7 @@
/*! \fn
\brief Get the actual joint speed
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
\param torque - desired speed (rad/s or m/s)
*/
public: PR2_ERROR_CODE GetJointSpeedActual(PR2_JOINT_ID id, double *speed);
@@ -224,7 +225,7 @@
/*! \fn
\brief Set the control mode for the arm
- \param id - modelId corresponding to the arm, set pr2Core.h for a list of model IDs
+ \param id - modelId corresponding to the arm, see pr2Core.h for a list of model IDs
\param mode - two choices (joint control or cartesian control)
*/
public: PR2_ERROR_CODE SetArmControlMode(PR2_MODEL_ID id, PR2_CONTROL_MODE mode);
@@ -232,7 +233,7 @@
/*! \fn
\brief Get the control mode for the arm
- \param id - modelId corresponding to the arm, set pr2Core.h for a list of model IDs
+ \param id - modelId corresponding to the arm, see pr2Core.h for a list of model IDs
\param mode - pointer to return value for mode, two choices (joint control or cartesian control)
*/
public: PR2_ERROR_CODE GetArmControlMode(PR2_MODEL_ID id, PR2_CONTROL_MODE *mode);
@@ -240,7 +241,7 @@
/*! \fn
\brief Enable the model (i.e. enable all actuators corresponding to a particular part of the robot)
- \param id - modelID, set pr2Core.h for a list of model IDs
+ \param id - modelID, see pr2Core.h for a list of model IDs
*/
public: PR2_ERROR_CODE EnableModel(PR2_MODEL_ID id);
@@ -263,14 +264,14 @@
/*! \fn
\brief Enable the arm (i.e. enable all actuators corresponding to an arm)
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
*/
public: PR2_ERROR_CODE EnableArm(PR2_MODEL_ID id);
/*! \fn
\brief Enable the gripper (i.e. enable all actuators corresponding to the gripper)
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
*/
public: PR2_ERROR_CODE EnableGripper(PR2_MODEL_ID id);
@@ -289,7 +290,7 @@
/*! \fn
\brief Disable the model (i.e. disable all actuators corresponding to a particular part of the robot)
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
*/
public: PR2_ERROR_CODE DisableModel(PR2_MODEL_ID id);
@@ -312,14 +313,14 @@
/*! \fn
\brief Disable the arm (i.e. disable all actuators corresponding to an arm)
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
*/
public: PR2_ERROR_CODE DisableArm(PR2_MODEL_ID id);
/*! \fn
\brief Disable the gripper (i.e. disable all actuators corresponding to a gripper)
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
*/
public: PR2_ERROR_CODE DisableGripper(PR2_MODEL_ID id);
@@ -338,7 +339,7 @@
/*! \fn
\brief Check whether all actuators in a particular part of the robot have been enabled
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param enabled - pointer to return value - 0 if any actuator in that part of the robot is disabled, 1 if all actuators in that part of the robot are enabled
*/
public: PR2_ERROR_CODE IsEnabledModel(PR2_MODEL_ID id, int *enabled);
@@ -346,7 +347,7 @@
/*! \fn
\brief Command the arm to go to a particular position in joint space
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param jointPosition - array of desired positions of all the joints
\param jointSpeed - array of desired speeds of all the joints
*/
@@ -355,7 +356,7 @@
/*! \fn
\brief Get the commanded joint values for an arm
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param jointPosition - array of desired positions of all the joints
\param jointSpeed - array of desired speeds of all the joints
*/
@@ -363,8 +364,27 @@
/*! \fn
+ \brief Get the actual wrist pose
+ \param id - model ID, see pr2Core.h for a list of model IDs
+ \param *x pointer to return value of x position of the wrist
+ \param *y pointer to return value of y position of the wrist
+ \param *z pointer to return value of z position of the wrist
+ \param *roll pointer to return value of roll of the wrist
+ \param *pitch pointer to return value of pitch of the wrist
+ \param *yaw pointer to return value of yaw of the wrist
+ */
+ public: PR2_ERROR_CODE GetWristPoseGroundTruth(PR2_MODEL_ID id, double *x, double *y, double *z, double *roll, double *pitch, double *yaw);
+
+
+ /*! \fn
+ \brief Get the actual wrist pose
+ \param id - model ID, see pr2Core.h for a list of model IDs
+ */
+ public: NEWMAT::Matrix GetWristPoseGroundTruth(PR2_MODEL_ID id);
+
+ /*! \fn
\brief Get the actual joint values for an arm
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param jointPosition - array of desired positions of all the joints
\param jointSpeed - array of desired speeds of all the joints
*/
@@ -373,14 +393,14 @@
/*! \fn
\brief Command a desired torque for all the joints in an arm
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param torque - array of desired torques for all the joints
*/
public: PR2_ERROR_CODE SetArmJointTorque(PR2_MODEL_ID id, double torque[]);
/*! \fn
\brief Get the commanded torque for all the joints in an arm
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param torque - array of desired torques for all the joints
*/
public: PR2_ERROR_CODE GetArmJointTorqueCmd(PR2_MODEL_ID id, double torque[]);
@@ -388,7 +408,7 @@
/*! \fn
\brief Get the actual torque for all the joints in an arm
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param torque - array of actual torques for all the joints
*/
public: PR2_ERROR_CODE GetArmJointTorqueActual(PR2_MODEL_ID id, double torque[]);
@@ -397,7 +417,7 @@
/*! \fn
\brief Command the arm to a particular speed in joint space
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param speed - array of desired speeds for all the joints
*/
public: PR2_ERROR_CODE SetArmJointSpeed(PR2_MODEL_ID id, double speed[]);
@@ -405,14 +425,14 @@
/*! \fn
\brief Get the commanded joint speeds for the entire arm
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param speed - array of desired speeds for all the joints
*/
public: PR2_ERROR_CODE GetArmJointSpeedCmd(PR2_MODEL_ID id, double speed[]);
/*! \fn
\brief Get the actual joint speeds for the entire arm
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param speed - array of desired speeds for all the joints
*/
public: PR2_ERROR_CODE GetArmJointSpeedActual(PR2_MODEL_ID id, double speed[]);
@@ -420,18 +440,16 @@
/*! \fn
\brief Command the end-effector to go to a particular position in cartesian space
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param effectorPosition - array of desired position of the end-effector
\param effectorSpeed - array of desired speed of the end-effector
*/
//public: PR2_ERROR_CODE SetArmCartesianPosition(PR2_MODEL_ID id, double effectorPosition[], double effectorSpeed[]);
public: PR2_ERROR_CODE SetArmCartesianPosition(PR2_MODEL_ID id, NEWMAT::Matrix g);
-
-#if 0
/*! \fn
\brief Get the commanded position and speed for the end-effector
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param effectorPosition - array of desired position of the end-effector
\param effectorSpeed - array of desired speed of the end-effector
*/
@@ -439,54 +457,69 @@
/*! \fn
\brief Get the actual cartesian position and speed for the end-effector
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param effectorPosition - array of desired position of the end-effector
\param effectorSpeed - array of desired speed of the end-effector
*/
public: PR2_ERROR_CODE GetArmCartesianPositionActual(PR2_MODEL_ID id, double effectorPosition[], double effectorSpeed[]);
/*! \fn
+ \brief Forward Kinematics - Compute cartesian position and speed for the end-effector
+ \param id - model ID, see pr2Core.h for a list of model IDs
+ \param angles - joint angles
+ */
+ public: NEWMAT::Matrix ComputeArmForwardKinematics(PR2_MODEL_ID id, double angles[]);
+
+ /*! \fn
+ \brief Inverse Kinematics - Compute joint angles corresponding to end-effector position and orientation
+ \param id - model ID, see pr2Core.h for a list of model IDs
+ \param g - representation of end-effector position and orientation
+ */
+ public: NEWMAT::Matrix ComputeArmInverseKinematics(PR2_MODEL_ID id, NEWMAT::Matrix g);
+
+ /*! \fn
\brief Command a desired end-effector force
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param force - desired end effector force
*/
public: PR2_ERROR_CODE SetArmCartesianForce(PR2_MODEL_ID id, double force[]);
/*! \fn
\brief Get the commanded end-effector force
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param force - desired end effector force
*/
public: PR2_ERROR_CODE GetArmCartesianForceCmd(PR2_MODEL_ID id, double force[]);
/*! \fn
\brief Get the actual end-effector force
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param force - desired end effector force
*/
public: PR2_ERROR_CODE GetArmCartesianForceActual(PR2_MODEL_ID id, double force[]);
/*! \fn
\brief Command a desired speed in cartesian space
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param speed - desired end effector speed
*/
public: PR2_ERROR_CODE SetArmCartesianSpeed(PR2_MODEL_ID id, double speed[]);
/*! \fn
\brief Get the commanded speed in cartesian space
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param force - desired end effector speed
*/
public: PR2_ERROR_CODE GetArmCartesianSpeedCmd(PR2_MODEL_ID id, double speed[]);
+
/*! \fn
\brief Get the actual end-effector speed in cartesian space
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param force - desired end effector speed
*/
public: PR2_ERROR_CODE GetArmCartesianSpeedActual(PR2_MODEL_ID id, double speed[]);
-#endif
+
/*! \fn
\brief Set a control mode for the base
\param mode - two choices - Cartesian or Joint control
@@ -510,6 +543,14 @@
public: PR2_ERROR_CODE SetBaseCartesianSpeedCmd(double vx, double vy, double vw);
/*! \fn
+ \brief Command a steering angle to achieve the desired speed. At the end of this maneuver the wheels will be at the steering angles required to achieve the desired combination of translational and rotational speeds.
+ \param vx - forward speed
+ \param vy - sideways speed
+ \param vw - rotational speed
+ */
+ public: PR2_ERROR_CODE SetBaseSteeringAngle(double vx, double vy, double vw);
+
+ /*! \fn
\brief Retrieve commanded speed for the base in cartesian space in body coordinates
\param vx - forward speed
\param vy - sideways speed
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-11 17:24:12 UTC (rev 742)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-11 17:49:55 UTC (rev 743)
@@ -1,9 +1,9 @@
+#include <libpr2API/pr2API.h>
+#include <math.h>
+
#include <gazebo/gazebo.h>
#include <gazebo/GazeboError.hh>
-#include <libpr2API/pr2API.h>
-#include <math.h>
-
using namespace gazebo;
using namespace PR2;
@@ -21,7 +21,10 @@
static gazebo::CameraIface *pr2CameraHeadLeftIface;
static gazebo::CameraIface *pr2CameraHeadRightIface;
+static gazebo::PositionIface *pr2LeftWristIface;
+static gazebo::PositionIface *pr2RightWristIface;
+
point Rot2D(point p,double theta)
{
point q;
@@ -109,6 +112,9 @@
pr2CameraHeadLeftIface = new gazebo::CameraIface();
pr2CameraHeadRightIface = new gazebo::CameraIface();
+ pr2LeftWristIface = new gazebo::PositionIface();
+ pr2RightWristIface = new gazebo::PositionIface();
+
int serverId = 0;
/// Connect to the libgazebo server
@@ -246,8 +252,29 @@
//return PR2_ERROR;
}
+ try
+ {
+ pr2LeftWristIface->Open(client, "p3d_left_wrist_position");
+ }
+ catch (std::string e)
+ {
+ std::cout << "Gazebo error: Unable to connect to the left wrist interface\n"
+ << e << "\n";
+ pr2LeftWristIface = NULL;
+ }
- return PR2_ALL_OK;
+ try
+ {
+ pr2RightWristIface->Open(client, "p3d_right_wrist_position");
+ }
+ catch (std::string e)
+ {
+ std::cout << "Gazebo error: Unable to connect to the right wrist interface\n"
+ << e << "\n";
+ pr2RightWristIface = NULL;
+ }
+
+ return PR2_ALL_OK;
}
PR2_ERROR_CODE PR2Robot::CalibrateRobot()
@@ -340,9 +367,8 @@
pr2Iface->Unlock();
}
return PR2_ALL_OK;
-}
+};
-
PR2_ERROR_CODE PR2Robot::EnableModel(PR2_MODEL_ID id)
{
if(IsHead(id))
@@ -584,6 +610,7 @@
g(2,4) = g(2,4) - SPINE_LEFT_ARM_OFFSET.y;
g(3,4) = g(3,4) - SPINE_LEFT_ARM_OFFSET.z;
}
+
theta = 0;
theta = myArm.ComputeIK(g,0.1);
for(int jj = 1; jj <= 8; jj++)
@@ -594,6 +621,7 @@
break;
}
}
+ validSolution = 1;
if(validSolution <= 8)
{
for(int ii = 0; ii < 7; ii++)
@@ -606,6 +634,54 @@
return PR2_ALL_OK;
};
+NEWMAT::Matrix PR2Robot::ComputeArmInverseKinematics(PR2_MODEL_ID id, NEWMAT::Matrix g)
+{
+ NEWMAT::Matrix theta(8,8);
+ double angles[7], speeds[7];
+ int validSolution;
+
+ if (id == PR2_RIGHT_ARM)
+ {
+ g(1,4) = g(1,4) - SPINE_RIGHT_ARM_OFFSET.x;
+ g(2,4) = g(2,4) - SPINE_RIGHT_ARM_OFFSET.y;
+ g(3,4) = g(3,4) - SPINE_RIGHT_ARM_OFFSET.z;
+ }
+
+ if (id == PR2_LEFT_ARM)
+ {
+ g(1,4) = g(1,4) - SPINE_LEFT_ARM_OFFSET.x;
+ g(2,4) = g(2,4) - SPINE_LEFT_ARM_OFFSET.y;
+ g(3,4) = g(3,4) - SPINE_LEFT_ARM_OFFSET.z;
+ }
+
+ theta = 0;
+ theta = myArm.ComputeIK(g,0.1);
+ return theta;
+};
+
+
+
+NEWMAT::Matrix PR2Robot::ComputeArmForwardKinematics(PR2_MODEL_ID id, double angles[])
+{
+ NEWMAT::Matrix g = myArm.ComputeFK(angles);
+
+ if (id == PR2_RIGHT_ARM)
+ {
+ g(1,4) = g(1,4) + SPINE_RIGHT_ARM_OFFSET.x;
+ g(2,4) = g(2,4) + SPINE_RIGHT_ARM_OFFSET.y;
+ g(3,4) = g(3,4) + SPINE_RIGHT_ARM_OFFSET.z;
+ }
+
+ if (id == PR2_LEFT_ARM)
+ {
+ g(1,4) = g(1,4) + SPINE_LEFT_ARM_OFFSET.x;
+ g(2,4) = g(2,4) + SPINE_LEFT_ARM_OFFSET.y;
+ g(3,4) = g(3,4) + SPINE_LEFT_ARM_OFFSET.z;
+ }
+ return g;
+};
+
+
PR2_ERROR_CODE PR2Robot::SetBaseControlMode(PR2_CONTROL_MODE mode)
{
baseControlMode = mode;
@@ -851,7 +927,7 @@
PR2_ERROR_CODE PR2Robot::SetArmJointPosition(PR2_MODEL_ID id, double jointPosition[], double jointSpeed[])
{
- if (id != PR2_RIGHT_ARM || id != PR2_LEFT_ARM)
+ if (id != PR2_RIGHT_ARM && id != PR2_LEFT_ARM)
return PR2_ERROR;
pr2Iface->Lock(1);
@@ -867,7 +943,7 @@
PR2_ERROR_CODE PR2Robot::GetArmJointPositionCmd(PR2_MODEL_ID id, double jointPosition[], double jointSpeed[])
{
- if (id != PR2_RIGHT_ARM || id != PR2_LEFT_ARM)
+ if (id != PR2_RIGHT_ARM && id != PR2_LEFT_ARM)
return PR2_ERROR;
pr2Iface->Lock(1);
@@ -883,14 +959,18 @@
PR2_ERROR_CODE PR2Robot::GetArmJointPositionActual(PR2_MODEL_ID id, double jointPosition[], double jointSpeed[])
{
- if (id != PR2_RIGHT_ARM || id != PR2_LEFT_ARM)
+ cout << "Entering joint positions " << endl << endl;
+
+ if (id != PR2_RIGHT_ARM && id != PR2_LEFT_ARM)
return PR2_ERROR;
+ cout << "Getting joint positions " << endl << endl;
pr2Iface->Lock(1);
for(int ii = JointStart[id]; ii <= JointEnd[id]; ii++)
{
jointPosition[ii-JointStart[id]] = pr2Iface->data->actuators[ii].actualPosition;
jointSpeed[ii-JointStart[id]] = pr2Iface->data->actuators[ii].actualSpeed;
+ cout << "ii" << (ii-JointStart[id]) << endl;
}
pr2Iface->Unlock();
return PR2_ALL_OK;
@@ -949,7 +1029,7 @@
PR2_ERROR_CODE PR2Robot::SetArmJointTorque(PR2_MODEL_ID id, double torque[])
{
- if (id != PR2_RIGHT_ARM || id != PR2_LEFT_ARM)
+ if (id != PR2_RIGHT_ARM && id != PR2_LEFT_ARM)
return PR2_ERROR;
pr2Iface->Lock(1);
@@ -961,7 +1041,7 @@
PR2_ERROR_CODE PR2Robot::GetArmJointTorqueCmd(PR2_MODEL_ID id, double torque[])
{
- if (id != PR2_RIGHT_ARM || id != PR2_LEFT_ARM)
+ if (id != PR2_RIGHT_ARM && id != PR2_LEFT_ARM)
return PR2_ERROR;
pr2Iface->Lock(1);
@@ -973,7 +1053,7 @@
PR2_ERROR_CODE PR2Robot::GetArmJointTorqueActual(PR2_MODEL_ID id, double torque[])
{
- if (id != PR2_RIGHT_ARM || id != PR2_LEFT_ARM)
+ if (id != PR2_RIGHT_ARM && id != PR2_LEFT_ARM)
return PR2_ERROR;
pr2Iface->Lock(1);
@@ -1034,7 +1114,7 @@
PR2_ERROR_CODE PR2Robot::SetArmJointSpeed(PR2_MODEL_ID id, double speed[])
{
- if (id != PR2_RIGHT_ARM || id != PR2_LEFT_ARM)
+ if (id != PR2_RIGHT_ARM && id != PR2_LEFT_ARM)
return PR2_ERROR;
pr2Iface->Lock(1);
for(int ii = JointStart[id]; ii <= JointEnd[id]; ii++)
@@ -1045,7 +1125,7 @@
PR2_ERROR_CODE PR2Robot::GetArmJointSpeedCmd(PR2_MODEL_ID id, double speed[])
{
- if (id != PR2_RIGHT_ARM || id != PR2_LEFT_ARM)
+ if (id != PR2_RIGHT_ARM && id != PR2_LEFT_ARM)
return PR2_ERROR;
pr2Iface->Lock(1);
for(int ii = JointStart[id]; ii <= JointEnd[id]; ii++)
@@ -1056,7 +1136,7 @@
PR2_ERROR_CODE PR2Robot::GetArmJointSpeedActual(PR2_MODEL_ID id, double speed[])
{
- if (id != PR2_RIGHT_ARM || id != PR2_LEFT_ARM)
+ if (id != PR2_RIGHT_ARM && id != PR2_LEFT_ARM)
return PR2_ERROR;
pr2Iface->Lock(1);
for(int ii = JointStart[id]; ii <= JointEnd[id]; ii++)
@@ -1115,6 +1195,36 @@
return PR2_ALL_OK;
};
+
+
+PR2_ERROR_CODE PR2Robot::SetBaseSteeringAngle(double vx, double vy, double vw)
+{
+ point steerPointVelocity[NUM_CASTERS];
+ double steerAngle[NUM_CASTERS];
+
+ point newDriveCenterL, newDriveCenterR;
+
+
+ for(int ii=0; ii < NUM_CASTERS; ii++)
+ {
+ ComputePointVelocity(vx,vy,vw,BASE_CASTER_OFFSET[ii].x,BASE_CASTER_OFFSET[ii].y,steerPointVelocity[ii].x,steerPointVelocity[ii].y);
+ steerAngle[ii] = atan2(steerPointVelocity[ii].y,steerPointVelocity[ii].x);
+ SetJointServoCmd((PR2_JOINT_ID) (CASTER_FL_STEER+3*ii),steerAngle[ii],0);
+ }
+
+ return PR2_ALL_OK;
+};
+
+
+
+
+
+
+
+
+
+
+
PR2_ERROR_CODE PR2Robot::GetBaseCartesianSpeedCmd(double* vx, double* vy, double* vw)
{
// FIXME: TODO: not implemented
@@ -1189,10 +1299,92 @@
*y = simIface->data->model_pose.pos.y;
*th = simIface->data->model_pose.yaw;
simIface->Unlock();
+ return PR2_ALL_OK;
+};
+
+PR2_ERROR_CODE PR2Robot::GetWristPoseGroundTruth(PR2_MODEL_ID id, double *x, double *y, double *z, double *roll, double *pitch, double *yaw)
+{
+ switch(id)
+ {
+ case PR2::PR2_LEFT_ARM:
+ pr2LeftWristIface->Lock(1);
+ *x = pr2LeftWristIface->data->pose.pos.x;
+ *y = pr2LeftWristIface->data->pose.pos.y;
+ *z = pr2LeftWristIface->data->pose.pos.z;
+ *roll = pr2LeftWristIface->data->pose.roll;
+ *pitch = pr2LeftWristIface->data->pose.pitch;
+ *yaw = pr2LeftWristIface->data->pose.yaw;
+ pr2LeftWristIface->Unlock();
+ break;
+ case PR2::PR2_RIGHT_ARM:
+ pr2RightWristIface->Lock(1);
+ *x = pr2RightWristIface->data->pose.pos.x;
+ *y = pr2RightWristIface->data->pose.pos.y;
+ *z = pr2RightWristIface->data->pose.pos.z;
+ *roll = pr2RightWristIface->data->pose.roll;
+ *pitch = pr2RightWristIface->data->pose.pitch;
+ *yaw = pr2RightWristIface->data->pose.yaw;
+ pr2RightWristIface->Unlock();
+ break;
+ default:
+ *x = 0;
+ *y = 0;
+ *z = 0;
+ *roll = 0;
+ *pitch = 0;
+ *yaw = 0;
+ return PR2_ERROR;
+ }
return PR2_ALL_OK;
};
+
+NEWMAT::Matrix PR2Robot::GetWristPoseGroundTruth(PR2_MODEL_ID id)
+{
+ NEWMAT::Matrix g(4,4);
+ double *x = new double[3];
+ double roll,pitch,yaw;
+ g = 0;
+ switch(id)
+ {
+ case PR2::PR2_LEFT_ARM:
+ pr2LeftWristIface->Lock(1);
+ x[0] = pr2LeftWristIface->data->pose.pos.x;
+ x[1] = pr2LeftWristIface->data->pose.pos.y;
+ x[2] = pr2LeftWristIface->data->pose.pos.z;
+ roll = pr2LeftWristIface->data->pose.roll;
+ pitch = pr2LeftWristIface->data->pose.pitch;
+ yaw = pr2LeftWristIface->data->pose.yaw;
+
+ pr2LeftWristIface->Unlock();
+ break;
+ case PR2::PR2_RIGHT_ARM:
+ pr2RightWristIface->Lock(1);
+ x[0] = pr2RightWristIface->data->pose.pos.x;
+ x[1] = pr2RightWristIface->data->pose.pos.y;
+ x[2] = pr2RightWristIface->data->pose.pos.z;
+ roll = pr2RightWristIface->data->pose.roll;
+ pitch = pr2RightWristIface->data->pose.pitch;
+ yaw = pr2RightWristIface->data->pose.yaw;
+ pr2RightWristIface->Unlock();
+ break;
+ default:
+ x[0] = 0;
+ x[1] = 0;
+ x[2] = 0;
+ roll = 0;
+ pitch = 0;
+ yaw = 0;
+ break;
+ }
+ cout << "Transform::" << x[0] << "," << x[1] << "," << x[2] << endl;
+ cout << "rpy::" << roll << "," << pitch << "," << yaw << endl;
+ g = Transform(x,roll,pitch,yaw);
+ delete x;
+ return g;
+};
+
PR2_ERROR_CODE PR2Robot::GetLaserRanges(PR2_SENSOR_ID id,
float* angle_min, float* angle_max, float* angle_increment,
float* range_max,uint32_t* ranges_size ,uint32_t* ranges_alloc_size,
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-11 17:24:12 UTC (rev 742)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_pr2API.cpp 2008-06-11 17:49:55 UTC (rev 743)
@@ -9,22 +9,17 @@
int main()
{
- // test pr2API
- PR2::PR2Robot myPR2;
+ // Test for pr2API
+ PR2::PR2Robot myPR2; // Setup the robot
+ myPR2.InitializeRobot(); // Initialize the robot (sets up kinematics, etc.)
- myPR2.InitializeRobot();
-
-
// set random numbers to base cartesian control
- myPR2.SetBaseControlMode(PR2_CARTESIAN_CONTROL);
- myPR2.SetBaseCartesianSpeedCmd(0.0,-0.5,1*M_PI/8);
+// myPR2.SetBaseControlMode(PR2_CARTESIAN_CONTROL);
+// myPR2.SetBaseCartesianSpeedCmd(0.0,-0.5,1*M_PI/8);
-
// test pitch the hokuyo
myPR2.SetJointServoCmd(PR2::HEAD_LASER_PITCH, -M_PI/8.0, 0.0);
-
-
// test kinematics library through pr2API
PR2Arm myArm;
@@ -34,7 +29,285 @@
// Joint angles (radians) and speeds for testing
double angles[7] = {0,0,0,0,0,0,0};
+ double speeds[7] = {0,0,0,0,0,0,0};
+ int iii;
+/*
+ angles[0] = 0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_RIGHT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[0] = -0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_RIGHT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[0] = -0;
+ myPR2.SetArmJointPosition(PR2::PR2_RIGHT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ cout << "Next joint?" << endl;
+ cin >> iii;
+
+
+ angles[1] = 0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_RIGHT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[1] = -0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_RIGHT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[1] = -0;
+ myPR2.SetArmJointPosition(PR2::PR2_RIGHT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ cout << "Next joint?" << endl;
+ cin >> iii;
+
+
+
+ angles[2] = 0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_RIGHT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[2] = -0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_RIGHT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[2] = -0;
+ myPR2.SetArmJointPosition(PR2::PR2_RIGHT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ cout << "Next joint?" << endl;
+ cin >> iii;
+
+
+ angles[3] = 0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_RIGHT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[3] = -0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_RIGHT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[3] = -0;
+ myPR2.SetArmJointPosition(PR2::PR2_RIGHT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ cout << "Next joint?" << endl;
+ cin >> iii;
+
+
+
+ angles[4] = 0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_RIGHT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[4] = -0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_RIGHT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[4] = -0;
+ myPR2.SetArmJointPosition(PR2::PR2_RIGHT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ cout << "Next joint?" << endl;
+ cin >> iii;
+
+
+ angles[5] = 0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_RIGHT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[5] = -0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_RIGHT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[5] = -0;
+ myPR2.SetArmJointPosition(PR2::PR2_RIGHT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ cout << "Next joint?" << endl;
+ cin >> iii;
+
+
+ angles[6] = 0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_RIGHT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[6] = -0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_RIGHT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[6] = -0;
+ myPR2.SetArmJointPosition(PR2::PR2_RIGHT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ cout << "Next joint?" << endl;
+ cin >> iii;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ angles[0] = 0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_LEFT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[0] = -0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_LEFT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[0] = -0;
+ myPR2.SetArmJointPosition(PR2::PR2_LEFT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ cout << "Next joint?" << endl;
+ cin >> iii;
+
+
+ angles[1] = 0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_LEFT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[1] = -0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_LEFT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[1] = -0;
+ myPR2.SetArmJointPosition(PR2::PR2_LEFT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ cout << "Next joint?" << endl;
+ cin >> iii;
+
+
+
+ angles[2] = 0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_LEFT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[2] = -0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_LEFT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[2] = -0;
+ myPR2.SetArmJointPosition(PR2::PR2_LEFT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ cout << "Next joint?" << endl;
+ cin >> iii;
+
+
+ angles[3] = 0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_LEFT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[3] = -0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_LEFT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[3] = -0;
+ myPR2.SetArmJointPosition(PR2::PR2_LEFT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ cout << "Next joint?" << endl;
+ cin >> iii;
+
+
+
+ angles[4] = 0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_LEFT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[4] = -0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_LEFT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[4] = -0;
+ myPR2.SetArmJointPosition(PR2::PR2_LEFT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ cout << "Next joint?" << endl;
+ cin >> iii;
+
+
+ angles[5] = 0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_LEFT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[5] = -0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_LEFT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[5] = -0;
+ myPR2.SetArmJointPosition(PR2::PR2_LEFT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ cout << "Next joint?" << endl;
+ cin >> iii;
+
+
+ angles[6] = 0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_LEFT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[6] = -0.5;
+ myPR2.SetArmJointPosition(PR2::PR2_LEFT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ angles[6] = -0;
+ myPR2.SetArmJointPosition(PR2::PR2_LEFT_ARM,angles,speeds); // send command to robot
+ sleep(4);
+
+ cout << "Next joint?" << endl;
+ cin >> iii;
+
+
+*/
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
angles[0] = 0.1; // shoulder pan angle
angles[1] = -1; // shoulder lift angle
angles[2] = 0.3; // upperarm roll angle
@@ -43,7 +316,43 @@
angles[5] = 0.5; // wrist pitch angle
angles[6] = 0.0; // wrist roll
- NEWMAT::Matrix pose = myArm.ComputeFK(angles);
+
+ g = myPR2.ComputeArmForwardKinematics(PR2::PR2_RIGHT_ARM,angles);
+ cout << "Main::End-effector Pose:" << endl << g << endl ;
+
+ // send command to robot
+ myPR2.SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,g);
+
+ NEWMAT::Matrix theta(8,8);
+ theta = 0;
+ theta = myPR2.ComputeArmInverseKinematics(PR2::PR2_RIGHT_ARM,g);
+ PrintMatrix(theta,"exact solution for pos/orien of end effector");
+
+ NEWMAT::Matrix g_wrist(4,4);
+ double x;
+ double y;
+ double z;
+ double roll;
+ double pitch;
+ double yaw;
+
+ myPR2.GetWristPoseGroundTruth(PR2::PR2_RIGHT_ARM,&x,&y,&z,&roll,&pitch,&yaw);
+ g_wrist = myPR2.GetWristPoseGroundTruth(PR2::PR2_RIGHT_ARM);
+
+ cout << "Right wrist::" << endl;
+ cout << "pos:: (" << x << "," << y << "," << z << ")" << endl;
+ cout << "rot:: (" << roll << "," << pitch << "," << yaw << ")" << endl;
+ PrintMatrix(g_wrist,"Wrist ground truth");
+
+ double jointPosition[7];
+ double jointSpeed[7];
+
+ myPR2.GetArmJointPositionActual(PR2::PR2_RIGHT_ARM,jointPosition,jointSpeed);
+
+ for(int ii=0; ii<7; ii++)
+ cout << "Joint ii::" << ii << ":: " << jointPosition[ii] << endl;
+
+/* NEWMAT::Matrix pose = myArm.ComputeFK(angles);
g = pose;
// offset from the base of the arm,
// subtracted for PR2 to cancel out effect of offset
@@ -52,7 +361,6 @@
g(2,4) = g(2,4) - PR2::SPINE_RIGHT_ARM_OFFSET.y;
g(3,4) = g(3,4) - PR2::SPINE_RIGHT_ARM_OFFSET.z;
-
// compute analytical solution
NEWMAT::Matrix theta(8,8);
theta = 0;
@@ -65,9 +373,14 @@
g(2,4) = g(2,4) + PR2::SPINE_RIGHT_ARM_OFFSET.y;
g(3,4) = g(3,4) + PR2::SPINE_RIGHT_ARM_OFFSET.z;
+ 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/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-06-11 17:24:12 UTC (rev 742)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-06-11 17:49:55 UTC (rev 743)
@@ -213,7 +213,7 @@
const point3 ELBOW_ROLL_WRIST_PITCH_OFFSET = {0.2237 ,0 ,0 };
const point3 WRIST_PITCH_WRIST_ROLL_OFFSET = {0 ,0 ,0 };
const point3 WRIST_ROLL_GRIPPER_OFFSET = {0 ,0 ,0 };
- const point3 SPINE_RIGHT_ARM_OFFSET = {0.0 , 0.15 , 0.68 };
- const point3 SPINE_LEFT_ARM_OFFSET = {0.0 , -0.15 , 0.68 };
+ const point3 SPINE_RIGHT_ARM_OFFSET = {0.0 , -0.15 , 0.68 };
+ const point3 SPINE_LEFT_ARM_OFFSET = {0.0 , 0.15 , 0.68 };
}
#endif
Modified: pkg/trunk/simulators/gazebo_plugin/Makefile
===================================================================
--- pkg/trunk/simulators/gazebo_plugin/Makefile 2008-06-11 17:24:12 UTC (rev 742)
+++ pkg/trunk/simulators/gazebo_plugin/Makefile 2008-06-11 17:49:55 UTC (rev 743)
@@ -12,8 +12,10 @@
LIB_GRP = lib/libPr2_Gripper.a
LIB_ACT_S = lib/libPr2_Actarray.so
LIB_GRP_S = lib/libPr2_Gripper.so
+LIB_P3D = lib/libP3D.a
+LIB_P3D_S = lib/libP3D.so
-all: $(LIB_ACT) $(LIB_GRP) $(LIB_ACT_S) $(LIB_GRP_S) #player_pr2
+all: $(LIB_ACT) $(LIB_GRP) $(LIB_ACT_S) $(LIB_GRP_S) $(LIB_P3D_S) $(LIB_P3D) #player_pr2
libPr2_Actarray.o: src/Pr2_Actarray.cc
@@ -22,18 +24,27 @@
libPr2_Gripper.o: src/Pr2_Gripper.cc
g++ $(CFLAGS) -o $@ -c $<
+libP3D.o: src/P3D.cc
+ g++ $(CFLAGS) -o $@ -c $<
+
$(LIB_ACT): libPr2_Actarray.o
ar -rs $@ $^
$(LIB_GRP): libPr2_Gripper.o
ar -rs $@ $^
+$(LIB_P3D): libP3D.o
+ ar -rs $@ $^
+
$(LIB_ACT_S): libPr2_Actarray.o
g++ -shared -o $@ $^ $(LDFLAGS)
$(LIB_GRP_S): libPr2_Gripper.o
g++ -shared -o $@ $^ $(LDFLAGS)
+$(LIB_P3D_S): libP3D.o
+ g++ -shared -o $@ $^ $(LDFLAGS)
+
player_pr2: src/player/Pr2_Player.cc
g++ $(CFLAGS) -o $@ $^ $(LFLAGS)
Modified: pkg/trunk/simulators/gazebo_plugin/SConstruct
===================================================================
--- pkg/trunk/simulators/gazebo_plugin/SConstruct 2008-06-11 17:24:12 UTC (rev 742)
+++ pkg/trunk/simulators/gazebo_plugin/SConstruct 2008-06-11 17:49:55 UTC (rev 743)
@@ -38,10 +38,11 @@
actarray_sharedLib = env.SharedLibrary('Pr2_Actarray','Pr2_Actarray.cc');
gripper_sharedLib = env.SharedLibrary('Pr2_Gripper' ,'Pr2_Gripper.cc' );
+P3D_sharedLib = env.SharedLibrary('P3D' ,'P3D.cc' );
env.Program('pr2','pr2.cc');
-env.Install(LIBROOT, [actarray_sharedLib,gripper_sharedLib]);
+env.Install(LIBROOT, [actarray_sharedLib,gripper_sharedLib,P3D_sharedLib]);
env.Install(INCROOT,headers);
Modified: pkg/trunk/simulators/gazebo_plugin/manifest.xml
===================================================================
--- pkg/trunk/simulators/gazebo_plugin/manifest.xml 2008-06-11 17:24:12 UTC (rev 742)
+++ pkg/trunk/simulators/gazebo_plugin/manifest.xml 2008-06-11 17:49:55 UTC (rev 743)
@@ -12,6 +12,6 @@
<depend package="pr2Core"/>
<depend package="newmat10"/>
<export>
-<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lPr2_Actarray -lPr2_Gripper"/>
+<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lPr2_Actarray -lPr2_Gripper -lP3D"/>
</export>
</package>
Modified: pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc
===================================================================
--- pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc 2008-06-11 17:24:12 UTC (rev 742)
+++ pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc 2008-06-11 17:49:55 UTC (rev 743)
@@ -20,8 +20,8 @@
*/
/*
* Desc: Actuator array controller for a Pr2 robot.
- * Author: Nathan Koenig
- * Date: 19 Sep 2007
+ * Author: Sachin Chitta and John Hsu
+ * Date: 1 June 2008
* SVN info: $Id$
*/
Modified: pkg/trunk/simulators/rosgazebo/world/pr2.model
===================================================================
--- pkg/trunk/simulators/rosgazebo/world/pr2.model 2008-06-11 17:24:12 UTC (rev 742)
+++ pkg/trunk/simulators/rosgazebo/world/pr2.model 2008-06-11 17:49:55 UTC (rev 743)
@@ -723,8 +723,8 @@
</joint:hinge>
<joint:hinge name="upperarm_roll_joint_right">
- <body1>upperarm_joint_right</body1>
- <body2>upperarm_right</body2>
+ <body1>upperarm_right</body1>
+ <body2>upperarm_joint_right</body2>
<anchor>upperarm_joint_right</anchor>
<axis> 1.0 0.0 0.0 </axis>
<cfm>0.001</cfm>
@@ -937,8 +937,8 @@
</joint:hinge>
<joint:hinge name="upperarm_roll_joint_left">
- <body1>upperarm_joint_left</body1>
- <body2>upperarm_left</body2>
+ <body1>upperarm_left</body1>
+ <body2>upperarm_joint_left</body2>
<anchor>upperarm_joint_left</anchor>
<axis> 1.0 0.0 0.0 </axis>
<cfm>0.001</cfm>
Modified: pkg/trunk/simulators/rosgazebo/world/robot.world
===================================================================
--- pkg/trunk/simulators/rosgazebo/world/robot.world 2008-06-11 17:24:12 UTC (rev 742)
+++ pkg/trunk/simulators/rosgazebo/world/robot.world 2008-06-11 17:49:55 UTC (rev 743)
@@ -593,10 +593,19 @@
<interface:pr2gripper name="pr2_gripper_right_iface"/>
</controller:pr2_gripper>
+ <controller:P3D name="p3d_right_wrist_controller" plugin="libP3D.so">
+ <bodyName>palm_right</bodyName>
+ <interface:position name="p3d_right_wrist_position"/>
+ </controller:P3D>
+
+ <controller:P3D name="p3d_left_wrist_controller" plugin="libP3D.so">
+ <bodyName>palm_left</bodyName>
+ <interface:position name="p3d_left_wrist_position"/>
+ </controller:P3D>
+
</model:physical>
-
<!-- White Directional light -->
<model:renderable name="directional_white">
<light>
@@ -609,6 +618,4 @@
</model:renderable>
-
-
</gazebo:world>
Modified: pkg/trunk/util/kinematics/libKinematics/include/libKinematics/kinematics.h
===================================================================
--- pkg/trunk/util/kinematics/libKinematics/include/libKinematics/kinematics.h 2008-06-11 17:24:12 UTC (rev 742)
+++ pkg/trunk/util/kinematics/libKinematics/include/libKinematics/kinematics.h 2008-06-11 17:49:55 UTC (rev 743)
@@ -101,6 +101,17 @@
NEWMAT::Matrix Translate(double p[]);
/*! \fn
+ \brief Generate the homogeneous transformation matrix corresponding to a translation and roll, pitch and yaw euler angles
+ \param p - translation vector (3 x 1)
+ \param roll - roll angle
+ \param pitch - pitch angle
+ \param yaw - yaw angle
+ \return Output homogeneous transformation matrix corresponding to a translation and roll, pitch and yaw euler angles
+ */
+ NEWMAT::Matrix Transform(double p[],double roll, double pitch, double yaw);
+
+
+ /*! \fn
\brief Get the position/translation part of a homogeneous transformation matrix
\param p - homogeneous transformation matrix
\return Output position vector
Modified: pkg/trunk/util/kinematics/libKinematics/src/ik.cpp
===================================================================
--- pkg/trunk/util/kinematics/libKinematics/src/ik.cpp 2008-06-11 17:24:12 UTC (rev 742)
+++ pkg/trunk/util/kinematics/libKinematics/src/ik.cpp 2008-06-11 17:49:55 UTC (rev 743)
@@ -12,13 +12,13 @@
PR2Arm::PR2Arm()
{
/* Link lengths */
- double p1[3] = {0,0,0};
- double p2[3] = {p1[0]+ARM_PAN_SHOULDER_PITCH_OFFSET.x,0,0};
- double p3[3] = {p2[0]+ARM_SHOULDER_PITCH_ROLL_OFFSET.x,0,0};
- double p4[3] = {p3[0]+ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.x,0,0};
- double p5[3] = {p4[0]+ELBOW_PITCH_ELBOW_ROLL_OFFSET.x,0,0};
- double p6[3] = {p5[0]+ELBOW_ROLL_WRIST_PITCH_OFFSET.x,0,0};
- double p7[3] = {p6[0]+WRIST_PITCH_WRIST_ROLL_OFFSET.x,0,0};
+ double p1[3] = {0,0,0};
+ double p2[3] = {p1[0]+ARM_PAN_SHOULDER_PITCH_OFFSET.x,p1[1],p1[2]};
+ double p3[3] = {p2[0]+ARM_SHOULDER_PITCH_ROLL_OFFSET.x,p2[1],p2[2]};
+ double p4[3] = {p3[0]+ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.x,p3[1],p3[2]};
+ double p5[3] = {p4[0]+ELBOW_PITCH_ELBOW_ROLL_OFFSET.x,p4[1],p4[2]};
+ double p6[3] = {p5[0]+ELBOW_ROLL_WRIST_PITCH_OFFSET.x,p5[1],p5[2]};
+ double p7[3] = {p6[0]+WRIST_PITCH_WRIST_ROLL_OFFSET.x,p6[1],p6[2]};
double wristDummy[3] = {0,0,0};
@@ -48,7 +48,6 @@
}
-
NEWMAT::Matrix PR2Arm::ComputeIK(NEWMAT::Matrix g, double theta1)
{
/*** A first solution using the shoulder rotation as a parameterization ***/
Modified: pkg/trunk/util/kinematics/libKinematics/src/kinematics.cpp
===================================================================
--- pkg/trunk/util/kinematics/libKinematics/src/kinematics.cpp 2008-06-11 17:24:12 UTC (rev 742)
+++ pkg/trunk/util/kinematics/libKinematics/src/kinematics.cpp 2008-06-11 17:49:55 UTC (rev 743)
@@ -143,6 +143,32 @@
return tM;
};
+
+NEWMAT::Matrix kinematics::Transform(double p[], double roll, double pitch, double yaw)
+{
+ NEWMAT::Matrix tM(4,4);
+ NEWMAT::IdentityMatrix I(4);
+ NEWMAT::Matrix xo(3,1),yo(3,1),zo(3,1);
+
+ tM = I;
+ xo = 0;
+ yo = 0;
+ zo = 0;
+
+ xo(1,1) = 1;
+ yo(2,1) = 1;
+ zo(3,1) = 1;
+
+ tM.SubMatrix(1,3,1,3) = ExpRot(zo,yaw)*ExpRot(yo,pitch)*ExpRot(xo,roll);
+
+ tM(1,4) = p[0];
+ tM(2,4) = p[1];
+ tM(3,4) = p[2];
+
+ return tM;
+};
+
+
NEWMAT::Matrix kinematics::TwistVectorToMatrix(const NEWMAT::Matrix& xi)
{
NEWMAT::Matrix xiHat(4,4);
@@ -516,7 +542,7 @@
NEWMAT::Matrix alpha,beta,gamma_numerator,gamma_denominator;
- double gamma,th11,th12,th21,th22;
+ double gamma;
p = pin.SubMatrix(1,3,1,1);
q = qin.SubMatrix(1,3,1,1);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
