Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [661] pkg/trunk
|
From: <hsu...@us...> - 2008-06-06 00:28:26
|
Revision: 661
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=661&view=rev
Author: hsujohnhsu
Date: 2008-06-05 17:28:26 -0700 (Thu, 05 Jun 2008)
Log Message:
-----------
added pr2Core definitions. added kinematics and inverse kinematics libraries.
Modified Paths:
--------------
pkg/trunk/simulators/rosgazebo/world/pr2.model
pkg/trunk/simulators/rosgazebo/world/robot.world
Added Paths:
-----------
pkg/trunk/drivers/robot/pr2/pr2Core/
pkg/trunk/drivers/robot/pr2/pr2Core/Makefile
pkg/trunk/drivers/robot/pr2/pr2Core/include/
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
pkg/trunk/drivers/robot/pr2/pr2Core/manifest.xml
pkg/trunk/util/kinematics/
pkg/trunk/util/kinematics/libKinematics/
pkg/trunk/util/kinematics/libKinematics/Makefile
pkg/trunk/util/kinematics/libKinematics/doc/
pkg/trunk/util/kinematics/libKinematics/include/
pkg/trunk/util/kinematics/libKinematics/include/libKinematics/
pkg/trunk/util/kinematics/libKinematics/include/libKinematics/ik.h
pkg/trunk/util/kinematics/libKinematics/include/libKinematics/kinematics.h
pkg/trunk/util/kinematics/libKinematics/lib/
pkg/trunk/util/kinematics/libKinematics/manifest.xml
pkg/trunk/util/kinematics/libKinematics/src/
pkg/trunk/util/kinematics/libKinematics/src/ik.cpp
pkg/trunk/util/kinematics/libKinematics/src/kinematics.cpp
pkg/trunk/util/kinematics/libKinematics/src/test/
pkg/trunk/util/kinematics/libKinematics/src/test/test_PR2ik.cpp
Added: pkg/trunk/drivers/robot/pr2/pr2Core/Makefile
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/Makefile (rev 0)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/Makefile 2008-06-06 00:28:26 UTC (rev 661)
@@ -0,0 +1,13 @@
+PKG =
+
+CFLAGS = -g -Wall -Iinclude `rospack export/cpp/cflags $(PKG)`
+
+LDFLAGS = `rospack export/cpp/lflags $(PKG)`
+
+LIB =
+
+all: $(LIB)
+
+clean:
+ #rm -f *.o *.a lib/*.a
+
Added: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h (rev 0)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-06-06 00:28:26 UTC (rev 661)
@@ -0,0 +1,213 @@
+// TODO: put enum in some Pr2 accessible file e.g. pr2.h and include it here
+#define MAX_JOINT_IDS 256
+
+namespace PR2
+{
+
+ enum PR2_JOINT_CONTROL_MODE{
+ TORQUE_CONTROL,
+ PD_CONTROL,
+ SPEED_CONTROL,
+ MAX_CONTROL_MODES
+ };
+
+ enum PR2_JOINT_TYPE{
+ PRISMATIC, ROTARY,
+ ROTARY_CONTINUOUS,
+ MAX_JOINT_TYPES
+ };
+
+ enum PR2_JOINT_ID{
+ CASTER_FL_STEER ,
+ CASTER_FL_DRIVE_L ,
+ CASTER_FL_DRIVE_R ,
+ CASTER_FR_STEER ,
+ CASTER_FR_DRIVE_L ,
+ CASTER_FR_DRIVE_R ,
+ CASTER_RL_STEER ,
+ CASTER_RL_DRIVE_L ,
+ CASTER_RL_DRIVE_R ,
+ CASTER_RR_STEER ,
+ CASTER_RR_DRIVE_L ,
+ CASTER_RR_DRIVE_R ,
+ SPINE_ELEVATOR ,
+ ARM_L_PAN ,
+ ARM_L_SHOULDER_PITCH,
+ ARM_L_SHOULDER_ROLL,
+ ARM_L_ELBOW_PITCH ,
+ ARM_L_ELBOW_ROLL ,
+ ARM_L_WRIST_PITCH ,
+ ARM_L_WRIST_ROLL ,
+ ARM_R_PAN ,
+ ARM_R_SHOULDER_PITCH,
+ ARM_R_SHOULDER_ROLL,
+ ARM_R_ELBOW_PITCH ,
+ ARM_R_ELBOW_ROLL ,
+ ARM_R_WRIST_PITCH ,
+ ARM_R_WRIST_ROLL ,
+ ARM_L_GRIPPER ,
+ ARM_R_GRIPPER ,
+ HEAD_YAW ,
+ HEAD_PITCH ,
+ HEAD_LASER_PITCH ,
+ HEAD_PTZ_L_PAN ,
+ HEAD_PTZ_L_TILT ,
+ HEAD_PTZ_R_PAN ,
+ HEAD_PTZ_R_TILT ,
+ MAX_JOINTS
+ };
+
+ enum PR2_SENSOR_ID{
+ CAMERA_GLOBAL ,
+ CAMERA_STEREO_LEFT ,
+ CAMERA_STEREO_RIGHT ,
+ CAMERA_HEAD_LEFT ,
+ CAMERA_HEAD_RIGHT ,
+ CAMERA_ARM_LEFT ,
+ CAMERA_ARM_RIGHT ,
+ LASER_HEAD ,
+ LASER_BASE ,
+ TACTILE_FINGER_LEFT ,
+ TACTILE_FINGER_RIGHT ,
+ MAX_SENSORS
+ };
+
+ enum PR2_BODY_ID{
+ CASTER_FL_WHEEL_L ,
+ CASTER_FL_WHEEL_R ,
+ CASTER_FL_BODY ,
+ CASTER_FR_WHEEL_L ,
+ CASTER_FR_WHEEL_R ,
+ CASTER_FR_BODY ,
+ CASTER_RL_WHEEL_L ,
+ CASTER_RL_WHEEL_R ,
+ CASTER_RL_BODY ,
+ CASTER_RR_WHEEL_L ,
+ CASTER_RR_WHEEL_R ,
+ CASTER_RR_BODY ,
+ BASE ,
+ TORSO ,
+ ARM_L_TURRET ,
+ ARM_L_SHOULDER ,
+ ARM_L_UPPERARM ,
+ ARM_L_ELBOW ,
+ ARM_L_FOREARM ,
+ ARM_L_WRIST ,
+ ARM_L_GRIPPER_TMP ,
+ ARM_L_FINGER_1 ,
+ ARM_L_FINGER_2 ,
+ ARM_R_TURRET ,
+ ARM_R_SHOULDER ,
+ ARM_R_UPPERARM ,
+ ARM_R_ELBOW ,
+ ARM_R_FOREARM ,
+ ARM_R_WRIST ,
+ ARM_R_GRIPPER_TMP ,
+ ARM_R_FINGER_1 ,
+ ARM_R_FINGER_2 ,
+ HEAD_BASE ,
+ LASER_BLOCK ,
+ STEREO_BLOCK ,
+ LASERBLOCK ,
+ MAX_BODY_IDS
+ };
+
+ enum PR2_JOINT_PARAM_ID {
+ P_GAIN,
+ I_GAIN,
+ D_GAIN,
+ I_CLAMP,
+ MAX_TORQUE
+ };
+
+ typedef struct
+ {
+ double timestamp;
+ double actualPosition;
+ double actualSpeed;
+ double actualEffectorForce;
+
+ int controlMode;
+ int jointType;
+
+ double cmdPosition;
+ double cmdSpeed;
+ double cmdEffectorForce;
+
+ int cmdEnableMotor;
+
+ double pGain;
+ double iGain;
+ double dGain;
+ double iClamp;
+ double saturationTorque;
+ } JointData;
+
+ enum PR2_ERROR_CODE {
+ PR2_ERROR,
+ PR2_ALL_OK,
+ PR2_MAX_ERROR_CODE
+ };
+
+ enum PR2_CONTROL_MODE {
+ PR2_CARTESIAN_CONTROL,
+ PR2_JOINT_CONTROL,
+ PR2_MAX_CONTROL_MODE
+ };
+
+ enum PR2_ROBOT_STATE {
+ STATE_INITIALIZED,
+ STATE_CALIBRATED,
+ STATE_RUNNING,
+ STATE_MOVING,
+ STATE_STOPPED,
+ MAX_ROBOT_STATES
+ };
+
+ typedef struct{
+ double x;
+ double y;
+ }point;
+
+ typedef struct{
+ double x;
+ double y;
+ double z;
+ }point3;
+
+ // JointStart/JointEnd corresponds to the PR2_MODEL_ID, start and end id for each model
+ enum PR2_MODEL_ID {PR2_BASE ,PR2_SPINE ,PR2_LEFT_ARM ,PR2_RIGHT_ARM ,PR2_LEFT_GRIPPER ,PR2_RIGHT_GRIPPER ,HEAD , MAX_MODELS };
+ const int JointStart[MAX_MODELS] = {CASTER_FL_STEER ,SPINE_ELEVATOR ,ARM_L_PAN ,ARM_R_PAN ,ARM_L_GRIPPER ,ARM_R_GRIPPER ,HEAD_YAW };
+ const int JointEnd[MAX_MODELS] = {CASTER_RR_DRIVE_R ,SPINE_ELEVATOR ,ARM_L_WRIST_ROLL ,ARM_R_WRIST_ROLL ,ARM_L_GRIPPER ,ARM_R_GRIPPER ,HEAD_LASER_PITCH };
+
+// Geometric description for the base
+ const float CASTER_FL_X_OFFSET = 0.25;
+ const float CASTER_FL_Y_OFFSET = 0.25;
+
+ const float CASTER_FR_X_OFFSET = 0.25;
+ const float CASTER_FR_Y_OFFSET = -0.25;
+
+ const float CASTER_RL_X_OFFSET = -0.25;
+ const float CASTER_RL_Y_OFFSET = 0.25;
+
+ const float CASTER_RR_X_OFFSET = -0.25;
+ const float CASTER_RR_Y_OFFSET = -0.25;
+
+ const float AXLE_WIDTH = 0.1;
+ const float WHEEL_RADIUS = 0.08;
+
+ const int NUM_CASTERS = 4;
+ const int NUM_WHEELS = 8;
+
+ const point BASE_CASTER_OFFSET[NUM_CASTERS] = {{0.25, 0.25}, {0.25, -0.25}, {-0.25,0.25}, {-0.25,-0.25}};
+ const point CASTER_DRIVE_OFFSET[NUM_WHEELS] = {{0,AXLE_WIDTH}, {0,-AXLE_WIDTH}, {0,AXLE_WIDTH}, {0,-AXLE_WIDTH},{0,AXLE_WIDTH}, {0,-AXLE_WIDTH},{0,AXLE_WIDTH}, {0,-AXLE_WIDTH}};
+
+ const point3 SPINE_ARM_PAN_OFFSET = {0,0,0};
+ const point3 ARM_PAN_SHOULDER_PITCH_OFFSET = {0.1,0,0};
+ const point3 ARM_SHOULDER_PITCH_ROLL_OFFSET = {0,0,0};
+ const point3 ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET = {0.4,0,0};
+ const point3 ELBOW_PITCH_ELBOW_ROLL_OFFSET = {0.09085,0,0};
+ 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};
+}
Added: pkg/trunk/drivers/robot/pr2/pr2Core/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/manifest.xml (rev 0)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/manifest.xml 2008-06-06 00:28:26 UTC (rev 661)
@@ -0,0 +1,13 @@
+<package>
+<description brief='PR2 Core Definitions for Joints, Bodies, Models'>
+
+Header file for PR2 core definitions of joints, bodies, models
+
+</description>
+<author>Sachin Chitta</author>
+<license>BSD</license>
+<url>http://pr.willowgarage.com</url>
+<export>
+<cpp cflags="-I${prefix}/include" lflags=""/>
+</export>
+</package>
Modified: pkg/trunk/simulators/rosgazebo/world/pr2.model
===================================================================
--- pkg/trunk/simulators/rosgazebo/world/pr2.model 2008-06-05 22:18:25 UTC (rev 660)
+++ pkg/trunk/simulators/rosgazebo/world/pr2.model 2008-06-06 00:28:26 UTC (rev 661)
@@ -374,7 +374,7 @@
<body2>dummytorso_body</body2>
<anchor>dummytorso_body</anchor>
<axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -383,7 +383,7 @@
<body2>dummytorso_body</body2>
<anchor>dummytorso_body</anchor>
<axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -582,7 +582,7 @@
<body2>torso_body</body2>
<anchor>shoulder_pan_body_right</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -591,7 +591,7 @@
<body2>shoulder_pan_body_right</body2>
<anchor>upperarm_joint_right</anchor>
<axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -600,7 +600,7 @@
<body2>upperarm_right</body2>
<anchor>upperarm_joint_right</anchor>
<axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -609,7 +609,7 @@
<body2>upperarm_right</body2>
<anchor>elbow_right</anchor>
<axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -618,7 +618,7 @@
<body2>elbow_right</body2>
<anchor>elbow_right</anchor>
<axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -627,7 +627,7 @@
<body2>forearm_right</body2>
<anchor>wrist_right</anchor>
<axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -636,7 +636,7 @@
<body2>wrist_right</body2>
<anchor>wrist_right</anchor>
<axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -796,7 +796,7 @@
<body2>torso_body</body2>
<anchor>shoulder_pan_body_left</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -805,7 +805,7 @@
<body2>shoulder_pan_body_left</body2>
<anchor>upperarm_joint_left</anchor>
<axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -814,7 +814,7 @@
<body2>upperarm_left</body2>
<anchor>upperarm_joint_left</anchor>
<axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -823,7 +823,7 @@
<body2>upperarm_left</body2>
<anchor>elbow_left</anchor>
<axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -832,7 +832,7 @@
<body2>elbow_left</body2>
<anchor>elbow_left</anchor>
<axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -841,7 +841,7 @@
<body2>forearm_left</body2>
<anchor>wrist_left</anchor>
<axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -850,7 +850,7 @@
<body2>wrist_left</body2>
<anchor>wrist_left</anchor>
<axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1079,8 +1079,6 @@
<body2>front_left_wheel_l</body2>
<anchor>front_left_wheel_l</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="front_left_wheel_r_drive">
@@ -1088,8 +1086,6 @@
<body2>front_left_wheel_r</body2>
<anchor>front_left_wheel_r</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="front_right_wheel_l_drive">
@@ -1097,8 +1093,6 @@
<body2>front_right_wheel_l</body2>
<anchor>front_right_wheel_l</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="front_right_wheel_r_drive">
@@ -1106,8 +1100,6 @@
<body2>front_right_wheel_r</body2>
<anchor>front_right_wheel_r</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="rear_left_wheel_l_drive">
@@ -1115,8 +1107,6 @@
<body2>rear_left_wheel_l</body2>
<anchor>rear_left_wheel_l</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="rear_left_wheel_r_drive">
@@ -1124,8 +1114,6 @@
<body2>rear_left_wheel_r</body2>
<anchor>rear_left_wheel_r</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="rear_right_wheel_l_drive">
@@ -1133,8 +1121,6 @@
<body2>rear_right_wheel_l</body2>
<anchor>rear_right_wheel_l</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="rear_right_wheel_r_drive">
@@ -1142,8 +1128,6 @@
<body2>rear_right_wheel_r</body2>
<anchor>rear_right_wheel_r</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
@@ -1154,8 +1138,6 @@
<body2>base_body</body2>
<anchor>front_left_caster_body</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="front_right_caster_steer">
@@ -1163,8 +1145,6 @@
<body2>base_body</body2>
<anchor>front_right_caster_body</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="rear_left_caster_steer">
@@ -1172,8 +1152,6 @@
<body2>base_body</body2>
<anchor>rear_left_caster_body</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="rear_right_caster_steer">
@@ -1181,8 +1159,6 @@
<body2>base_body</body2>
<anchor>rear_right_caster_body</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
Modified: pkg/trunk/simulators/rosgazebo/world/robot.world
===================================================================
--- pkg/trunk/simulators/rosgazebo/world/robot.world 2008-06-05 22:18:25 UTC (rev 660)
+++ pkg/trunk/simulators/rosgazebo/world/robot.world 2008-06-06 00:28:26 UTC (rev 661)
@@ -23,9 +23,9 @@
<!-- cfm is 1e-5 for single precision -->
<!-- erp is typically .1-.8 -->
<physics:ode>
- <stepTime>0.01</stepTime>
+ <stepTime>0.001</stepTime>
<gravity>0 0 -9.8</gravity>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.8</erp>
</physics:ode>
Added: pkg/trunk/util/kinematics/libKinematics/Makefile
===================================================================
--- pkg/trunk/util/kinematics/libKinematics/Makefile (rev 0)
+++ pkg/trunk/util/kinematics/libKinematics/Makefile 2008-06-06 00:28:26 UTC (rev 661)
@@ -0,0 +1,31 @@
+PKG = libKinematics
+
+CFLAGS = -g -Wall -Iinclude `rospack export/cpp/cflags $(PKG)`
+
+LDFLAGS = `rospack export/cpp/lflags $(PKG)`
+
+LIBK = lib/libKinematics.a
+LIBIK = lib/libIk.a
+
+all: $(LIBK) $(LIBIK) test_PR2ik
+
+
+ik.o: src/ik.cpp include/libKinematics/ik.h include/libKinematics/kinematics.h
+ g++ $(CFLAGS) -o $@ -c $<
+
+kinematics.o: src/kinematics.cpp include/libKinematics/kinematics.h
+ g++ $(CFLAGS) -o $@ -c $<
+
+$(LIBK): kinematics.o
+ ar -rs $@ $^
+
+$(LIBIK): ik.o kinematics.o
+ ar -rs $@ $^
+
+test_PR2ik: src/test/test_PR2ik.cpp $(LIBK) $(LIBIK)
+ g++ $(CFLAGS) -o $@ $^ $(LDFLAGS)
+
+clean:
+ rm -rf test_PR2ik
+ rm -f *.o *.a lib/*.a
+
Added: pkg/trunk/util/kinematics/libKinematics/include/libKinematics/ik.h
===================================================================
--- pkg/trunk/util/kinematics/libKinematics/include/libKinematics/ik.h (rev 0)
+++ pkg/trunk/util/kinematics/libKinematics/include/libKinematics/ik.h 2008-06-06 00:28:26 UTC (rev 661)
@@ -0,0 +1,57 @@
+//Software License Agreement (BSD License)
+
+//Copyright (c) 2008, Willow Garage, Inc.
+//All rights reserved.
+
+//Redistribution and use in source and binary forms, with or without
+//modification, are permitted provided that the following conditions
+//are met:
+
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following
+// disclaimer in the documentation and/or other materials provided
+// with the distribution.
+// * Neither the name of the Willow Garage nor the names of its
+// contributors may be used to endorse or promote products derived
+// from this software without specific prior written permission.
+
+//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+//"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+//LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+//FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+//COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+//INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+//BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+//LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+//CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+//LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+//ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+//POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef PR2_KINEMATICS_HH
+#define PR2_KINEMATICS_HH
+
+#include <iostream>
+#include <newmat10/newmat.h>
+#include <newmat10/newmatio.h>
+#include <libKinematics/kinematics.h>
+
+using namespace kinematics;
+
+namespace PR2
+{
+ class PR2Arm : public kinematics::SerialRobot
+ {
+ public:
+
+ PR2Arm();
+
+ NEWMAT::Matrix ComputeIK(NEWMAT::Matrix g, double theta1);
+
+ SerialRobot Wrist;
+ };
+}
+
+#endif
Added: pkg/trunk/util/kinematics/libKinematics/include/libKinematics/kinematics.h
===================================================================
--- pkg/trunk/util/kinematics/libKinematics/include/libKinematics/kinematics.h (rev 0)
+++ pkg/trunk/util/kinematics/libKinematics/include/libKinematics/kinematics.h 2008-06-06 00:28:26 UTC (rev 661)
@@ -0,0 +1,357 @@
+//Software License Agreement (BSD License)
+
+//Copyright (c) 2008, Willow Garage, Inc.
+//All rights reserved.
+
+//Redistribution and use in source and binary forms, with or without
+//modification, are permitted provided that the following conditions
+//are met:
+
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following
+// disclaimer in the documentation and/or other materials provided
+// with the distribution.
+// * Neither the name of the Willow Garage nor the names of its
+// contributors may be used to endorse or promote products derived
+// from this software without specific prior written permission.
+
+//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+//"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+//LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+//FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+//COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+//INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+//BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+//LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+//CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+//LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+//ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+//POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef KINEMATICS_HH
+#define KINEMATICS_HH
+
+#define MAX_NUM_JOINTS 64
+
+#include <iostream>
+#include <newmat10/newmat.h>
+#include <newmat10/newmatio.h>
+
+namespace kinematics
+{
+ /*! \fn
+ \brief Transform a twist from one coordinate frame to another
+ \param g - NEWMAT matrix representing transformation,
+ \param xi - twist vector that needs to be transformed,
+ \return NEWMAT matrix representing the transformed twist vector.
+ */
+ NEWMAT::Matrix TransformTwist(const NEWMAT::Matrix& g, const NEWMAT::Matrix& xi);
+
+ /*! \fn
+ \brief Return the twist matrix corresponding to a twist vector
+ \param xi - input twist vector (as a NEWMAT matrix)
+ \return NEWMAT matrix representing the transformed twist vector.
+ */
+ NEWMAT::Matrix TwistVectorToMatrix(const NEWMAT::Matrix& xi);
+
+ /*! \fn
+ \brief Convert twist matrix to vector representation
+ \param xiHat - input twist in matrix form (as a NEWMAT matrix)
+ \return NEWMAT vector representing the transformed twist matrix
+ */
+ NEWMAT::Matrix TwistMatrixToVector(const NEWMAT::Matrix& xiHat);
+
+ /*! \fn
+ \brief Compute the transform corresponding to the exponential of a twist
+ \param xi - input twist vector (as a NEWMAT matrix)
+ \param theta - rotation angle/translational distance
+ \return NEWMAT matrix corresponding to the matrix exponential
+ */
+ NEWMAT::Matrix ExpTwist(NEWMAT::Matrix xi, double theta);
+
+ /*! \fn
+ \brief Compute the rotation matrix corresponding to rotation about an axis by a desired angle
+ \param omega - axis of rotation
+ \param theta - angle of rotation
+ \return Output NEWMAT rotation matrix
+ */
+ NEWMAT::Matrix ExpRot(NEWMAT::Matrix omega, double theta);
+
+ /*! \fn
+ \brief Get the (3 x 1) axis corresponding to a twist vector
+ \param xi - input twist vector (as a NEWMAT matrix)
+ \return Output NEWMAT axis vector (3 x 1)
+ */
+ NEWMAT::Matrix GetAxis(NEWMAT::Matrix xi);
+
+ /*! \fn
+ \brief Get the (3 x 1) speed part of a twist vector
+ \param xi - input twist vector (as a NEWMAT matrix)
+ \return Output NEWMAT speed vector (3 x 1)
+ */
+ NEWMAT::Matrix GetSpeed(NEWMAT::Matrix xi);
+
+ /*! \fn
+ \brief Generate the homogeneous transformation matrix corresponding to a translation
+ \param p - translation vector (3 x 1)
+ \return Output homogeneous transformation matrix corresponding to a translation
+ */
+ NEWMAT::Matrix Translate(double p[]);
+
+ /*! \fn
+ \brief Get the position/translation part of a homogeneous transformation matrix
+ \param p - homogeneous transformation matrix
+ \return Output position vector
+ */
+ NEWMAT::Matrix GetPosition(const NEWMAT::Matrix& p);
+
+ /*! \fn
+ \brief Get the rotation matrix part of a homogeneous transformation matrix
+ \param p - homogeneous transformation matrix
+ \return Output rotation matrix
+ */
+ NEWMAT::Matrix GetRotationMatrix(const NEWMAT::Matrix& p);
+
+ /*! \fn
+ \brief Get the skew-symmetric matrix corresponding to a rotation axis
+ \param omega - (3 x 1) vector representing a rotation axis
+ \return Output skew-symmetric rotation matrix
+ */
+ NEWMAT::Matrix GetHatMatrix(NEWMAT::Matrix omega);
+
+ /*! \fn
+ \brief Compute the cross product of two vectors
+ \param p1 - NEWMAT matrix representation of the first vector
+ \param p2 - NEWMAT matrix representation of the second vector
+ \return Output skew-symmetric rotation matrix
+ */
+ NEWMAT::Matrix cross(NEWMAT::Matrix p1, NEWMAT::Matrix p2);
+
+ /*! \fn
+ \brief Compute the cross product of two vectors
+ \param p1 - array representation of the first vector
+ \param p2 - array representation of the second vector
+ \param p3 - array representation of the resultant vector from the cross-product
+ */
+ void cross(const double p1[], const double p2[], double p3[]);
+
+ /*! \fn
+ \brief Compute the magnitude of a vector
+ \param omega - input vector (as a NEWMAT matrix)
+ */
+ double GetNorm(NEWMAT::Matrix omega);
+
+ /*! \fn
+\brief Solution of the first form of the Paden-Kahan subproblems. This formulation solves for theta in exp(xi theta) p = q
+\param p NEWMAT matrix representation of a point on which the exponential operates
+\param q NEWMAT matrix representation of the resultant point after rotation through thets
+\param r point on the axis about which the rotation is happening
+\param omega direction vector for the axis about which the rotation is happening. The twist vector xi is a combination of this vector and a linear velocity term v.
+\return theta solution to the above expression
+ */
+ double SubProblem1(NEWMAT::Matrix p, NEWMAT::Matrix q, NEWMAT::Matrix r, NEWMAT::Matrix omega);
+
+
+ /*! \fn
+\brief Solution of the first form of the Paden-Kahan subproblems. This formulation solves for theta in exp(xi1 theta1) exp(xi2 theta2) p = q
+\param p NEWMAT matrix representation of a point on which the exponential operates
+\param q NEWMAT matrix representation of the resultant point after rotation through thets
+\param r point on the axis about which the rotation is happening
+\param omega direction vector for the axis about which the rotation is happening. The twist vectors xi1 and xi2 are a combination of this vector and linear velocity terms v1 and v2.
+\param theta[] a 4x1 vector containing the two solutions to this problem.
+ */
+ void SubProblem2(NEWMAT::Matrix pin, NEWMAT::Matrix qin, NEWMAT::Matrix rin, NEWMAT::Matrix omega1, NEWMAT::Matrix omega2, double theta[]);
+
+
+/*! \fn
+\brief Print the matrix in a nice form
+\param m Matrix to be printed
+\param c string representing matrix specification
+*/
+ void PrintMatrix(NEWMAT::Matrix m, std::string c);
+
+
+/*! \enum PR2_JOINT_TYPE
+ * Joint types
+*/
+ enum PR2_JOINT_TYPE{
+ PRISMATIC,
+ ROTARY,
+ ROTARY_CONTINUOUS,
+ MAX_JOINT_TYPES
+ };
+
+ class Joint
+ {
+ public:
+
+/*! \fn
+ Default constructor
+*/
+ Joint(){};
+
+/*! \fn
+ Default destructor
+*/
+ virtual ~Joint(){};
+
+ int jointId; /**< joint ID */
+
+ NEWMAT::Matrix linkPose; /**< link pose */
+
+ NEWMAT::Matrix twist;; /**< link twist */
+ };
+
+ /*! \class
+ \brief The serial robot class allows the construction of a serial manipulator using a series of links and joints
+ */
+ class SerialRobot
+ {
+
+ public:
+
+ /*! \fn
+ \brief Construct a serial robot with n joints
+ \param nJoints - number of joints
+ */
+ SerialRobot(int nJoints);
+
+ SerialRobot();
+
+ /*! \fn
+ \brief Default destructor
+ */
+ virtual ~SerialRobot();
+
+ /*! \fn
+ \brief Initialize the joint structure of the serial robot
+ \param nJoints - number of joints
+ */
+ void Initialize(int nJoints);
+
+ Joint *joints; /**< pointer to set of joints in the robot */
+
+ /*! \fn
+ \brief Add a new joint to the serial robot,
+ \param joint - twist vector corresponding to the joint
+ */
+ void AddJoint(NEWMAT::Matrix joint, double p[]);
+
+ /*! \fn
+ \brief Add a new joint to the serial robot,
+ \param joint - twist vector corresponding to the joint
+ */
+ void AddJoint(double p[], double axis[], int jointType);
+
+ /*! \fn
+ \brief Compute the inverse kinematics for a serial manipulator (NOT IMPLEMENTED YET)
+ \param p - homogeneous transformation matrix representing an end-effector pose
+ \param jointangle - return array of computed joint angles
+ */
+ void ComputeIK(NEWMAT::Matrix p, double jointAngles[]);
+
+ /*! \fn
+ \brief Get the COG pose for a particular body (NOT IMPLEMENTED YET)
+ */
+ void GetCOGPose(int id);
+
+ /*! \fn
+ \brief Get the pose for a particular link in the serial manipulator
+ \param id - body/link ID
+ \param jointangle - input array of joint angles
+ */
+ NEWMAT::Matrix GetPose(int id, double jointAngles[]);
+
+ /*! \fn
+ \brief Get the joint twist for a particular joint in the serial manipulator
+ \param id - body/link ID
+ \return twist vector for the joint
+ */
+ NEWMAT::Matrix GetTwist(int id);
+
+ /*! \fn
+ \brief Get the joint axis for a particular joint in the serial manipulator
+ \param id - body/link ID
+ \return axis for the joint
+ */
+ NEWMAT::Matrix GetJointAxis(int id);
+
+ /*! \fn
+ \brief Get the location of a point on the joint axis for a particular joint in the serial manipulator IN THE HOME POSITION
+ \param id - body/link ID
+ \return axis for the joint
+ */
+ NEWMAT::Matrix GetJointAxisPoint(int id);
+
+ /*! \fn
+ \brief Get the exponential corresponding to the joint twist for a particular joint
+ \param id - joint id
+ \param theta - joint angle
+ \return twist vector for the joint
+ */
+ NEWMAT::Matrix GetJointExponential(int id, double theta);
+
+
+ /*! \fn
+ \brief Compute the forward kinematics given a particular set of joint angles
+ \return Homogeneous transformation matrix corresponding to the pose of the end effector
+ */
+ NEWMAT::Matrix ComputeFK(double jointAngles[]);
+
+ /*! \fn
+ \brief Compute the forward kinematics upto a particular link/body given a particular set of joint angles
+ \param jointAngles - array of joint angles
+ \param bodyId - Id for a body part/link
+ \return Homogeneous transformation matrix corresponding to the pose of a link/body
+ */
+ NEWMAT::Matrix ComputeFK(double jointAngles[], int bodyId);
+
+ /*! \fn
+ \brief Compute the manipulator jacobian matrix given a particular set of joint angles
+ \param jointAngles - array of joint angles
+ \return Manipulator jacobian matrix
+ */
+ NEWMAT::Matrix ComputeManipulatorJacobian(double jointAngles[]);
+
+ /*! \fn
+ \brief Get the pose of a particular link/body
+ \param angles - array of joint angles
+ \return Get the homogeneous transformation matrix corresponding to the pose of a link/body
+ */
+ NEWMAT::Matrix GetLinkPose(int id, double angles[]);
+
+ /*! \fn
+ \brief Compute the end effector velocity given a set of joint angles and joint speeds.
+ \param jointAngles - array of joint angles
+ \param jointSpeeds - array of joint speeds
+ \return end effector velocity corresponding to the given joint angles and joint speeds
+ */
+ NEWMAT::Matrix ComputeEndEffectorVelocity(double jointAngles[], double jointSpeeds[]);
+
+ /*! \fn
+ \brief Return the default/home position of the end-effector.
+ \return Homogeneous matrix representation of the default/home position of the PR2
+ */
+ NEWMAT::Matrix GetHomePosition();
+
+ /*! \fn
+ \brief Set the default/home position of the end-effector.
+ \return Homogeneous matrix representation of the default/home position of the PR2
+ */
+ void SetHomePosition(NEWMAT::Matrix g);
+
+ protected:
+
+ NEWMAT::Matrix homePosition; /**< Homogeneous matrix representation of the default/home position of the PR2 */
+
+ private:
+
+ int numberJoints; /**< number of joints in the manipulators */
+
+ int jointCount; /**< internal counter */
+ };
+}
+
+#endif
Added: pkg/trunk/util/kinematics/libKinematics/manifest.xml
===================================================================
--- pkg/trunk/util/kinematics/libKinematics/manifest.xml (rev 0)
+++ pkg/trunk/util/kinematics/libKinematics/manifest.xml 2008-06-06 00:28:26 UTC (rev 661)
@@ -0,0 +1,15 @@
+<package>
+<description brief='Kinematics Library'>
+
+Library for doing kinematics and inverse kinematics for the PR2 robot
+
+</description>
+<author>Sachin Chitta</author>
+<license>BSD</license>
+<url>http://pr.willowgarage.com</url>
+<depend package="newmat10"/>
+<depend package="pr2Core"/>
+<export>
+<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lKinematics -lIk -lnewmat"/>
+</export>
+</package>
Added: pkg/trunk/util/kinematics/libKinematics/src/ik.cpp
===================================================================
--- pkg/trunk/util/kinematics/libKinematics/src/ik.cpp (rev 0)
+++ pkg/trunk/util/kinematics/libKinematics/src/ik.cpp 2008-06-06 00:28:26 UTC (rev 661)
@@ -0,0 +1,296 @@
+#include <libKinematics/kinematics.h>
+#include <pr2Core/pr2Core.h>
+#include <math.h>
+#include <libKinematics/ik.h>
+
+double a1x(0.1),a2x(0.5),a3x(0.59085),a4x(0.81455),a5x(0.81455);
+using namespace kinematics;
+using namespace PR2;
+using namespace std;
+
+
+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 wristDummy[3] = {0,0,0};
+
+/* Axis definitions */
+ double axis1[3] = {0,0,1};
+ double axis2[3] = {0,1,0};
+ double axis3[3] = {1,0,0};
+ double axis4[3] = {0,1,0};
+ double axis5[3] = {1,0,0};
+ double axis6[3] = {0,1,0};
+ double axis7[3] = {1,0,0};
+
+/* Joint angles and speeds for testing */
+ double angles[7] = {0,0,0,0,0,0,0};
+
+ this->Initialize(7);
+ this->AddJoint(p1,axis1,kinematics::ROTARY);
+ this->AddJoint(p2,axis2,kinematics::ROTARY);
+ this->AddJoint(p3,axis3,kinematics::ROTARY);
+ this->AddJoint(p4,axis4,kinematics::ROTARY);
+ this->AddJoint(p5,axis5,kinematics::ROTARY);
+ this->AddJoint(p6,axis6,kinematics::ROTARY);
+ this->AddJoint(p7,axis7,kinematics::ROTARY);
+
+ NEWMAT::Matrix g0 = this->GetLinkPose(7,angles);
+ this->SetHomePosition(g0);
+
+}
+
+
+NEWMAT::Matrix PR2Arm::ComputeIK(NEWMAT::Matrix g, double theta1)
+{
+ /*** A first solution using the shoulder rotation as a parameterization ***/
+ /* Find shoulder offset point */
+ int ii, jj, kk, ll;
+
+ double t1,t2,t3,t4,t5,t6,t7;
+
+ NEWMAT::Matrix sop(3,1), distance(3,1);
+ NEWMAT::Matrix g0 = this->GetHomePosition();
+ NEWMAT::Matrix g0Wrist = this->GetHomePosition();
+ NEWMAT::Matrix theta(8,8);
+
+ NEWMAT::Matrix xi1,xi2,xi3,xi4,xi5,xi6,xi7;
+ NEWMAT::Matrix p,q,r;
+ NEWMAT::Matrix omega1,omega2,omega3,omega4,omega5,omega6;
+ NEWMAT::Matrix pWrist,qWrist,rWrist;
+ NEWMAT::Matrix q0;
+
+ NEWMAT::Matrix theta2(2,1),theta3(2,1),theta4(2,1),theta5(2,1),theta6(2,1);
+
+ double dd, numerator, denominator, acosTerm;
+
+ int solutionCount = 1;
+
+ theta = 0;
+ t1 = theta1;
+
+ sop(1,1) = a1x*cos(t1);
+ sop(2,1) = a1x*sin(t1);
+ sop(3,1) = 0;
+
+ distance = g.SubMatrix(1,3,4,4) - sop;
+ dd = pow(distance.NormFrobenius(),2);
+
+#ifdef DEBUG
+ PrintMatrix(distance,"ComputeIK::distance::");
+ cout << "ComputeIK::dd::" << dd << endl;
+ cout << "ComputeIK::a1x::" << a1x << ", a2x::" << a2x << ", a3x::" << a3x << ", a4x::" << a4x << endl;
+#endif
+
+ numerator = dd-a1x*a1x+2*a1x*a2x-2*a2x*a2x+2*a2x*a4x-a4x*a4x;
+ denominator = 2*(a1x-a2x)*(a2x-a4x);
+
+ acosTerm = numerator/denominator;
+
+ if (acosTerm > 1.0)
+ acosTerm = 1.0;
+ if (acosTerm < -1.0)
+ acosTerm = -1.0;
+
+ theta4(1,1) = acos(acosTerm);
+ theta4(2,1) = -acos(acosTerm);
+
+#ifdef DEBUG
+ cout << "ComputeIK::theta3:" << numerator << "," << denominator << "," << endl << theta[3] << endl;
+ PrintMatrix(theta4,"theta4");/* There are now two solution streams */
+#endif
+
+ for(jj =1; jj <= 2; jj++)
+ {
+ t4 = theta4(jj,1);
+
+ /* Start solving for the other angles */
+ q0 = g0.SubMatrix(1,4,4,4);
+ q0(3,1) = q0(3,1);
+
+ xi1 = this->GetJointExponential(0,t1);
+ xi4 = this->GetJointExponential(3,t4);
+ xi5 = this->GetJointExponential(4,0);
+ xi6 = this->GetJointExponential(5,0);
+ xi7 = this->GetJointExponential(6,0);
+
+ q = xi1.i()*g*g0.i()*xi7.i()*xi6.i()*xi5.i()*q0;
+ p = xi4*q0;
+ r = this->GetJointAxisPoint(1);
+
+ omega1 = this->GetJointAxis(1);
+ omega2 = this->GetJointAxis(2);
+
+#ifdef DEBUG
+ PrintMatrix(g0,"ComputeIK::g0");
+ PrintMatrix(q0,"ComputeIK::q0");
+ PrintMatrix(omega1,"ComputeIK::omega1");
+ PrintMatrix(omega2,"ComputeIK::omega2");
+
+ PrintMatrix(g,"ComputeIK::g");
+ PrintMatrix(p,"ComputeIK::p");
+ PrintMatrix(q,"ComputeIK::q");
+ PrintMatrix(r,"ComputeIK::r");
+
+ PrintMatrix(xi1,"ComputeIK::xi1");
+ PrintMatrix(xi2,"ComputeIK::xi2");
+ PrintMatrix(xi3,"ComputeIK::xi3");
+ PrintMatrix(xi4,"ComputeIK::xi4");
+ PrintMatrix(xi5,"ComputeIK::xi5");
+ PrintMatrix(xi6,"ComputeIK::xi6");
+ PrintMatrix(xi7,"ComputeIK::xi7");
+#endif
+
+ double *theta_ans = new double[4];
+ SubProblem2(p,q,r,omega1,omega2,theta_ans);
+
+ theta2(1,1) = theta_ans[0];
+ theta2(2,1) = theta_ans[2];
+
+ theta3(1,1) = theta_ans[1];
+ theta3(2,1) = theta_ans[3];
+
+#ifdef DEBUG
+ PrintMatrix(theta2,"theta2");
+ PrintMatrix(theta3,"theta3");
+#endif
+
+ for(kk=1; kk <= 2; kk++)
+ {
+ t2 = theta2(kk,1);
+ t3 = theta3(kk,1);
+ /* Two more solutions here, making for a total of 4 solution stream so far */
+
+#ifdef DEBUG
+ PrintMatrix(theta,"Answer");
+#endif
+
+ /* Solve for the wrist angles now. First, take everything but the wrist over to the lhs.
+ Use this information to solve for the 5th and 6th axes.
+ */
+ xi2 = this->GetJointExponential(1,t2);
+ xi3 = this->GetJointExponential(2,t3);
+
+ rWrist = this->GetJointAxisPoint(6);
+ pWrist = rWrist;
+ pWrist(1,1) = pWrist(1,1) + 0.1;
+ qWrist = xi4.i()*xi3.i()*xi2.i()*xi1.i()*g*g0.i()*pWrist;
+
+ omega4 = this->GetJointAxis(4);
+ omega5 = this->GetJointAxis(5);
+ omega6 = this->GetJointAxis(6);
+
+ SubProblem2(pWrist,qWrist,rWrist,omega4,omega5,theta_ans);
+
+ /* Two more solutions here, making for a total of 8 solution streams so far */
+ theta5(1,1) = theta_ans[0];
+ theta6(1,1) = theta_ans[1];
+ theta5(2,1) = theta_ans[2];
+ theta6(2,1) = theta_ans[3];
+
+#ifdef DEBUG
+ PrintMatrix(theta5,"theta5");
+ PrintMatrix(theta6,"theta6");
+#endif
+
+ for (ll=1; ll <=2; ll++)
+ {
+ t5 = theta5(ll,1);
+ t6 = theta6(ll,1);
+
+ xi5 = this->GetJointExponential(4,t5);
+ xi6 = this->GetJointExponential(5,t6);
+ /* Now use these solutions to solve for the 7th axis */
+ pWrist(3,1) = pWrist(3,1) + 0.1;
+ qWrist = xi6.i()*xi5.i()*xi4.i()*xi3.i()*xi2.i()*xi1.i()*g*g0.i()*pWrist;
+ t7 = SubProblem1(pWrist,qWrist,rWrist,omega6);
+
+ theta(1,solutionCount) = t1;
+ theta(2,solutionCount) = t2;
+ theta(3,solutionCount) = t3;
+ theta(4,solutionCount) = t4;
+ theta(5,solutionCount) = t5;
+ theta(6,solutionCount) = t6;
+ theta(7,solutionCount) = t7;
+
+ if (isnan(t1) || isnan(t2) || isnan(t3) || isnan(t4) || isnan(t5) || isnan(t6) || isnan(t7))
+ theta(8,solutionCount) = -1;
+#ifdef DEBUG
+ cout << "t1: " << t1 << ", t2: " << t2 << ", t3: " << t3 << ", t4: " << t4 << ", t5: " << t5 << ", t6: " << t6 << ", t7: " << t7 << endl;
+#endif
+ solutionCount++;
+ }
+ }
+ }
+ return theta;
+}
+
+
+/**** List of angles (for reference) *******
+th1 = shoulder/turret pan
+th2 = shoulder/turret lift/pitch
+th3 = shoulder/turret roll
+th4 = elbow pitch
+th5 = elbow roll
+th6 = wrist pitch
+th7 = wrist roll
+
+ NEWMAT::Matrix gWrist(4,4);
+ NEWMAT::Matrix pWrist(4,1);
+ NEWMAT::Matrix rWrist(4,1);
+
+ pWrist(1,1) = 0.1;
+ pWrist(2,1) = 0;
+ pWrist(3,1) = 0;
+ pWrist(4,1) = 1;
+
+ rWrist(1,1) = 0;
+ rWrist(2,1) = 0;
+ rWrist(3,1) = 0;
+ rWrist(4,1) = 1;
+
+ gWrist = g;
+
+ gWrist(1,4) = 0;
+ gWrist(2,4) = 0;
+ gWrist(3,4) = 0;
+
+ double *theta_wrist = new double[2];
+
+ NEWMAT::Matrix qWrist = gWrist*g0Wrist.i()*pWrist;
+
+ NEWMAT::Matrix wristxi3 = Wrist.GetJointExponential(2,0);
+
+ NEWMAT::Matrix omega4 = this->GetJointAxis(4);
+ NEWMAT::Matrix omega5 = this->GetJointAxis(5);
+ NEWMAT::Matrix omega6 = this->GetJointAxis(6);
+
+ SubProblem2(pWrist,qWrist,rWrist,omega4,omega5,theta_wrist);
+
+ theta(5,1) = theta_wrist[0];
+ theta(6,1) = theta_wrist[1];
+
+ NEWMAT::Matrix wristxi1 = Wrist.GetJointExponential(0,theta(5,1));
+ NEWMAT::Matrix wristxi2 = Wrist.GetJointExponential(1,theta(6,1));
+
+ NEWMAT::Matrix rhs = wristxi2.i()*wristxi1.i()*gWrist*g0Wrist.i()*pWrist;
+ theta(7,1) = SubProblem1(pWrist,rhs,rWrist,omega6);
+
+ double wristAngles[3] = {0,0,0};
+ Wrist.Initialize(3);
+ Wrist.AddJoint(wristDummy,axis5,kinematics::ROTARY);
+ Wrist.AddJoint(wristDummy,axis6,kinematics::ROTARY);
+ Wrist.AddJoint(wristDummy,axis7,kinematics::ROTARY);
+
+ NEWMAT::Matrix g0Wrist = this->GetLinkPose(3,wristAngles);
+ Wrist.SetHomePosition(g0Wrist);
+
+****/
Added: pkg/trunk/util/kinematics/libKinematics/src/kinematics.cpp
===================================================================
--- pkg/trunk/util/kinematics/libKinematics/src/kinematics.cpp (rev 0)
+++ pkg/trunk/util/kinematics/libKinematics/src/kinematics.cpp 2008-06-06 00:28:26 UTC (rev 661)
@@ -0,0 +1,577 @@
+#include <math.h>
+#include <pr2Core/pr2Core.h>
+#include <libKinematics/kinematics.h>
+
+#define EPS 0.000001
+using namespace kinematics;
+
+void kinematics::PrintMatrix(NEWMAT::Matrix m, std::string c)
+{
+ cout << c << endl << m << endl << endl;
+}
+
+NEWMAT::Matrix kinematics::GetHatMatrix(NEWMAT::Matrix omega)
+{
+ NEWMAT::Matrix result(3,3);
+
+ result = 0;
+
+ result(1,2) = -omega(3,1);
+ result(1,3) = omega(2,1);
+ result(2,1) = omega(3,1);
+ result(2,3) = -omega(1,1);
+ result(3,1) = -omega(2,1);
+ result(3,2) = omega(1,1);
+
+ return result;
+};
+
+NEWMAT::Matrix kinematics::ExpRot(NEWMAT::Matrix omega, double theta)
+{
+ NEWMAT::Matrix Rot(3,3);
+ NEWMAT::IdentityMatrix I(3);
+ NEWMAT::Matrix omegaHat(3,3);
+ double omegaMag;
+
+ omegaHat = GetHatMatrix(omega);
+ omegaMag = GetNorm(omega);
+
+ if (omegaMag < EPS)
+ Rot = I;
+ else
+ Rot = I + omegaHat*(sin(omegaMag*theta)/omegaMag) + (omegaHat*omegaHat)*(1-cos(omegaMag*theta))/(omegaMag*omegaMag);
+#ifdef DEBUG
+ cout << omega;
+ cout << "ExpRot" << endl << Rot << endl;
+#endif
+ return Rot;
+};
+
+double kinematics::GetNorm(NEWMAT::Matrix omega)
+{
+ double norm(0);
+ norm = sqrt(omega(1,1)*omega(1,1)+omega(2,1)*omega(2,1)+omega(3,1)*omega(3,1));
+ return norm;
+};
+
+NEWMAT::Matrix kinematics::ExpTwist(NEWMAT::Matrix xi, double theta)
+{
+ NEWMAT::Matrix Rot(3,3);
+ NEWMAT::Matrix v(3,1);
+ NEWMAT::Matrix omega(3,1);
+ NEWMAT::IdentityMatrix I(3);
+ NEWMAT::Matrix result(4,4);
+ NEWMAT::Matrix Trans(3,1);
+ double omegaMag;
+
+ omega = GetAxis(xi);
+ v = GetSpeed(xi);
+ Rot = ExpRot(omega,theta);
+
+ omegaMag = GetNorm(omega);
+
+ if (omegaMag < EPS)
+ Trans = v*theta;
+ else
+ Trans = (I - Rot)*cross(omega,v) + omega*omega.t()*v*theta;
+
+
+ result = 0;
+
+ result(1,1) = Rot(1,1);
+ result(1,2) = Rot(1,2);
+ result(1,3) = Rot(1,3);
+
+ result(2,1) = Rot(2,1);
+ result(2,2) = Rot(2,2);
+ result(2,3) = Rot(2,3);
+
+ result(3,1) = Rot(3,1);
+ result(3,2) = Rot(3,2);
+ result(3,3) = Rot(3,3);
+
+ result(1,4) = Trans(1,1);
+ result(2,4) = Trans(2,1);
+ result(3,4) = Trans(3,1);
+ result(4,4) = 1;
+
+#ifdef DEBUG
+ cout << "ExpTwist: v : " << endl << v << endl;
+#endif
+
+ return result;
+};
+
+NEWMAT::Matrix kinematics::GetAxis(NEWMAT::Matrix xi)
+{
+ NEWMAT::Matrix omega;
+ omega = xi.SubMatrix(4,6,1,1);
+ return omega;
+}
+
+NEWMAT::Matrix kinematics::GetSpeed(NEWMAT::Matrix xi)
+{
+ NEWMAT::Matrix v;
+ v = xi.SubMatrix(1,3,1,1);
+ return v;
+}
+
+NEWMAT::Matrix kinematics::Translate(double p[])
+{
+ NEWMAT::Matrix tM(4,4);
+
+ tM(1,1) = 1;
+ tM(1,2) = 0;
+ tM(1,3) = 0;
+ tM(1,4) = p[0];
+
+ tM(2,1) = 0;
+ tM(2,2) = 1;
+ tM(2,3) = 0;
+ tM(2,4) = p[1];
+
+ tM(3,1) = 0;
+ tM(3,2) = 0;
+ tM(3,3) = 1;
+ tM(3,4) = p[2];
+
+ tM(4,1) = 0;
+ tM(4,2) = 0;
+ tM(4,3) = 0;
+ tM(4,4) = 1;
+
+ return tM;
+};
+
+NEWMAT::Matrix kinematics::TwistVectorToMatrix(const NEWMAT::Matrix& xi)
+{
+ NEWMAT::Matrix xiHat(4,4);
+ xiHat = 0;
+ xiHat(1,1) = 0;
+ xiHat(1,2) = -xi(6,1);
+ xiHat(1,3) = -xi(5,1);
+
+ xiHat(2,1) = xi(6,1);
+ xiHat(2,2) = 0;
+ xiHat(2,3) = -xi(4,1);
+
+ xiHat(3,1) = -xi(5,1);
+ xiHat(3,2) = -xi(4,1);
+ xiHat(3,3) = 0;
+
+ xiHat(1,4) = xi(1,1);
+ xiHat(2,4) = xi(2,1);
+ xiHat(3,4) = xi(3,1);
+ xiHat(4,4) = 1;
+
+ return xiHat;
+};
+
+NEWMAT::Matrix kinematics::TwistMatrixToVector(const NEWMAT::Matrix& xiHat)
+{
+ NEWMAT::Matrix xi(6,1);
+#ifdef DEBUG
+ cout << "TwistMatrixToVector::" << xiHat << endl;
+#endif
+ xi(1,1) = xiHat(1,4);
+ xi(2,1) = xiHat(2,4);
+ xi(3,1) = xiHat(3,4);
+
+ xi(4,1) = -xiHat(2,3);
+ xi(5,1) = -xiHat(3,1);
+ xi(6,1) = -xiHat(1,2);
+
+ return xi;
+};
+
+NEWMAT::Matrix kinematics::TransformTwist(const NEWMAT::Matrix& xi, const NEWMAT::Matrix& g)
+{
+ NEWMAT::Matrix xi_new(6,1);
+ NEWMAT::Matrix tM = kinematics::TwistVectorToMatrix(xi);
+
+ xi_new = kinematics::TwistMatrixToVector(g*tM*g.i());
+
+#ifdef DEBUG
+ cout << "TransformTwist::xi" << xi << endl;
+ cout << "TransformTwist::tM" << tM << endl;
+ cout << "TransformTwist::xi_new" << xi_new << endl;
+#endif
+
+ return xi_new;
+};
+
+
+NEWMAT::Matrix kinematics::GetPosition(const NEWMAT::Matrix& p)
+{
+ NEWMAT::Matrix q(4,1);
+
+ q.SubMatrix(1,3,1,1) = p.SubMatrix(1,3,4,4);
+ q(4,1) = 1;
+ return q;
+};
+
+NEWMAT::Matrix kinematics::GetRotationMatrix(const NEWMAT::Matrix& p)
+{
+ NEWMAT::Matrix q(3,3);
+
+ q = p.SubMatrix(1,3,1,3);
+ return q;
+};
+
+SerialRobot::SerialRobot(int nJoints)
+{
+ NEWMAT::IdentityMatrix I(4);
+ jointCount = 0;
+ homePosition = I;
+ Initialize(nJoints);
+};
+
+SerialRobot::SerialRobot()
+{
+ NEWMAT::IdentityMatrix I(4);
+ numberJoints = 0;
+ jointCount = 0;
+ homePosition = I;
+};
+
+void SerialRobot::Initialize(int nJoints)
+{
+ numberJoints = nJoints;
+ joints = new Joint[numberJoints];
+}
+
+SerialRobot::~SerialRobot()
+{
+ joints = NULL;
+};
+
+void SerialRobot::SetHomePosition(NEWMAT::Matrix g)
+{
+ homePosition = g;
+}
+
+NEWMAT::Matrix SerialRobot::GetHomePosition()
+{
+ return homePosition;
+}
+
+void SerialRobot::AddJoint(NEWMAT::Matrix jin, double p[])
+{
+// *(joints[jointCount]) = new NEWMAT::Matrix(6,1);
+ joints[jointCount].twist = jin;
+ joints[jointCount].jointId = jointCount;
+ joints[jointCount].linkPose = Translate(p);
+#ifdef DEBUG
+ cout << "AddJoint:: " << joints[jointCount].linkPose << endl;
+ cout << "Link pose" << endl << joints[jointCount].linkPose << endl;
+ cout << "twist " << endl << joints[jointCount].twist << endl;
+#endif
+ jointCount++;
+};
+
+void kinematics::cross(const double p1[], const double p2[], double p3[])
+{
+ p3[0] = p1[1]*p2[2]-p1[2]*p2[1];
+ p3[1] = p1[2]*p2[0]-p1[0]*p2[2];
+ p3[2] = p1[0]*p2[1]-p1[1]*p2[0];
+
+#if DEBUG
+ printf("p1: %f %f %f\n",p1[0],p1[1],p1[2]);
+ printf("p2: %f %f %f\n",p2[0],p2[1],p2[2]);
+ printf("p3: %f %f %f\n",p3[0],p3[1],p3[2]);
+#endif
+};
+
+NEWMAT::Matrix kinematics::cross(NEWMAT::Matrix p1, NEWMAT::Matrix p2)
+{
+ NEWMAT::Matrix p3(3,1);
+ p3(1,1) = p1(2,1)*p2(3,1)-p1(3,1)*p2(2,1);
+ p3(2,1) = p1(3,1)*p2(1,1)-p1(1,1)*p2(3,1);
+ p3(3,1) = p1(1,1)*p2(2,1)-p1(2,1)*p2(1,1);
+
+ return p3;
+};
+
+void SerialRobot::AddJoint(double p[], double axis[], int jointType)
+{
+ NEWMAT::Matrix jj(6,1);
+ switch(jointType)
+ {
+ case PRISMATIC:
+ jj(1,1) = axis[0];
+ jj(2,1) = axis[1];
+ jj(3,1) = axis[2];
+ jj(4,1) = 0;
+ jj(5,1) = 0;
+ jj(6,1) = 0;
+#ifdef DEBUG
+ printf("Joint type: PRISMATIC\n");
+#endif
+ break;
+ case ROTARY:
+ double q[3];
+ cross(p,axis,q);
+ jj(1,1) = q[0];
+ jj(2,1) = q[1];
+ jj(3,1) = q[2];
+ jj(4,1) = axis[0];
+ jj(5,1) = axis[1];
+ jj(6,1) = axis[2];
+#ifdef DEBUG
+ printf("Joint type: ROTARY\n");
+#endif
+ break;
+ default:
+ break;
+ };
+ AddJoint(jj,p);
+};
+
+NEWMAT::Matrix SerialRobot::GetLinkPose(int id, double angles[])
+{
+ NEWMAT::Matrix returnPose;
+ NEWMAT::IdentityMatrix I(4);
+ if(id ==0)//ground link
+ returnPose = I;
+ else
+ returnPose = ComputeFK(angles,id-1);
+
+ return returnPose;
+};
+
+NEWMAT::Matrix SerialRobot::GetTwist(int id)
+{
+ return joints[id].twist;
+};
+
+NEWMAT::Matrix SerialRobot::GetJointExponential(int id, double theta)
+{
+#ifdef DEBUG
+ cout << "id:" << id << endl << joints[id].twist << endl << theta << endl;
+#endif
+ return ExpTwist(joints[id].twist,theta);
+};
+
+NEWMAT::Matrix SerialRobot::GetJointAxisPoint(int id)
+{
+ return GetPosition(joints[id].linkPose);
+};
+
+NEWMAT::Matrix SerialRobot::GetJointAxis(int id)
+{
+ return GetAxis(joints[id].twist);
+};
+
+
+NEWMAT::Matrix SerialRobot::ComputeFK(double jointAngles[])
+{
+ NEWMAT::Matrix returnPose;
+ returnPose = ComputeFK(jointAngles,numberJoints-1);
+
+ return returnPose;
+};
+
+NEWMAT::Matrix SerialRobot::ComputeFK(double jointAngles[], int id)
+{
+ NEWMAT::Matrix returnPose(4,4);
+ NEWMAT::IdentityMatrix I(4);
+
+ returnPose = I;
+
+ for(int ii = 0; ii <= id; ii++)
+ {
+ returnPose = returnPose * ExpTwist(joints[ii].twist,jointAngles[ii]);
+#ifdef DEBUG
+ cout << "ii: " << ii << endl;
+ cout << "ComputeFK::" << "Intermediate return pose" << endl << returnPose << endl;
+ cout << "ComputeFK::" << "Twist exponential" << endl << ExpTwist(joints[ii].twist,jointAngles[ii]) << endl;
+#endif
+ }
+
+#ifdef DEBUG
+ cout << "ComputeFK::" << "link Pose " << endl << joints[id].linkPose << endl;
+ cout << "returnPose::" << endl << returnPose << endl;
+#endif
+ if(id == (numberJoints-1))
+ returnPose = returnPose * joints[id].linkPose;
+ else
+ returnPose = returnPose * joints[id].linkPose;
+
+ return returnPose;
+};
+
+void SerialRobot::ComputeIK(NEWMAT::Matrix p, double jointAngles[]){};
+
+NEWMAT::Matrix SerialRobot::ComputeManipulatorJacobian(double jointAngles[])
+{
+//
+// Each column of the jacobian is the twist vector for that joint in the new configuration frame
+// The resultant matrix represents the Manipulator jacobian (and not the traditional jacobian)
+//
+// A good reference for this is the book: "A Mathematical Introduction to Robotic Manipulation"
+// by Richard Murray, Zexiang Li and Shankar Sastry
+//
+ NEWMAT::Matrix iP;
+ NEWMAT::Matrix jacobian(6,numberJoints);
+ NEWMAT::Matrix newTwist(6,1);
+ NEWMAT::IdentityMatrix I(4);
+
+ iP = I;
+
+ for(int ii = 0; ii < numberJoints; ii++)
+ {
+ newTwist = TransformTwist(joints[ii].twist,iP);
+ iP = iP*ExpTwist(joints[ii].twist,jointAngles[ii]);
+
+ jacobian(1,ii+1) = newTwist(1,1);
+ jacobian(2,ii+1) = newTwist(2,1);
+ jacobian(3,ii+1) = newTwist(3,1);
+ jacobian(4,ii+1) = newTwist(4,1);
+ jacobian(5,ii+1) = newTwist(5,1);
+ jacobian(6,ii+1) = newTwist(6,1);
+ };
+ return jacobian;
+};
+
+NEWMAT::Matrix SerialRobot::ComputeEndEffectorVelocity(double jointAngles[], double jointSpeeds[])
+{
+ NEWMAT::Matrix jacobian(6,numberJoints);
+ NEWMAT::Matrix speed(numberJoints,1);
+ NEWMAT::Matrix spatialVelocity(numberJoints,1);
+ NEWMAT::Matrix spatialVelocityMatrix(4,4);
+ NEWMAT::Matrix endEffectorVelocity(6,1);
+ NEWMAT::Matrix endEffectorTVelocity(4,1);
+ NEWMAT::Matrix endEffectorPosition(4,1);
+
+ /* Initialize everything to zero */
+ jacobian = 0;
+ speed = 0;
+ spatialVelocity = 0;
+ spatialVelocityMatrix = 0;
+
+ speed << jointSpeeds;
+
+ endEffectorPosition = GetPosition(ComputeFK(jointAngles));
+ jacobian = ComputeManipulatorJacobian(jointAngles);
+ spatialVelocity = jacobian*speed;
+ spatialVelocityMatrix = TwistVectorToMatrix(spatialVelocity);
+ endEffectorTVelocity = spatialVelocityMatrix * endEffectorPosition;
+
+ endEffectorVelocity(1,1) = endEffectorTVelocity(1,1);
+ endEffectorVelocity(2,1) = endEffectorTVelocity(2,1);
+ endEffectorVelocity(3,1) = endEffectorTVelocity(3,1);
+
+ endEffectorVelocity(4,1) = spatialVelocity(4,1);
+ endEffectorVelocity(5,1) = spatialVelocity(5,1);
+ endEffectorVelocity(6,1) = spatialVelocity(6,1);
+
+ return endEffectorVelocity;
+};
+
+double kinematics::SubProblem1(NEWMAT::Matrix pin, NEWMAT::Matrix qin, NEWMAT::Matrix rin, NEWMAT::Matrix w)
+{
+ NEWMAT::Matrix u(3,1),v(3,1),up(3,1),vp(3,1);
+ NEWMAT::Matrix p(3,1),q(3,1),r(3,1);
+
+ NEWMAT::Matrix arg1, arg2;
+
+ p = pin.SubMatrix(1,3,1,1);
+ q = qin.SubMatrix(1,3,1,1);
+ r = rin.SubMatrix(1,3,1,1);
+
+#ifdef DEBUG
+ PrintMatrix(p,"SubProblem1::p");
+ PrintMatrix(q,"SubProblem1::q");
+ PrintMatrix(r,"SubProblem1::r");
+ PrintMatrix(w,"SubProblem1::w");
+#endif
+
+ u = p-r;
+ v = q-r;
+
+ up = u - w*(w.t()*u);
+ vp = v - w*(w.t()*v);
+
+#ifdef DEBUG
+ PrintMatrix(up,"SP1::up::");
+ PrintMatrix(vp,"SP1::vp::");
+ PrintMatrix(w,"SP1::w");
+#endif
+
+ arg1 = w.t()*cross(up,vp);
+ arg2 = up.t()*vp;
+
+#ifdef DEBUG
+ cout << "SubProblem1::" << arg1(1,1) << "," << arg2(1,1) << endl;
+ cout << endl << endl << endl;
+#endif
+ return atan2(arg1(1,1),arg2(1,1));
+}
+
+void kinematics::SubProblem2(NEWMAT::Matrix pin, NEWMAT::Matrix qin, NEWMAT::Matrix rin, NEWMAT::Matrix omega1, NEWMAT::Matrix omega2, double theta[])
+{
+ NEWMAT::Matrix u,v,z,c;
+ NEWMAT::Matrix p,q,r;
+ NEWMAT::Matrix denom;
+
+ NEWMAT::Matrix alpha,beta,gamma_numerator,gamma_denominator;
+
+ double gamma,th11,th12,th21,th22;
+
+ p = pin.SubMatrix(1,3,1,1);
+ q = qin.SubMatrix(1,3,1,1);
+ r = rin.SubMatrix(1,3,1,1);
+
+ u = p-r;
+ v = q-r;
+
+ denom = omega1.t()*omega2;
+
+#ifdef DEBUG
+ PrintMatrix(p,"SubProblem2::p");
+ PrintMatrix(q,"SubProblem2::q");
+ PrintMatrix(u,"SubProblem2::u");
+ PrintMatrix(v,"SubProblem2::v");
+ PrintMatrix(denom,"SubProblem2::denom");
+#endif
+
+ alpha = ((omega1.t()*omega2)*(omega2.t()*u)-(omega1.t()*v))/(pow(denom(1,1),2)-1);
+ beta = ((omega1.t()*omega2)*(omega1.t()*v)-(omega2.t()*u))/(pow(denom(1,1),2)-1);
+
+#ifdef DEBUG
+ PrintMatrix(alpha,"SubProblem2::alpha");
+ PrintMatrix(beta,"SubProblem2::beta");
+#endif
+
+ gamma_numerator = ((u.t()*u)-pow(alpha(1,1),2)-pow(beta(1,1),2)-2*alpha(1,1)*beta(1,1)*(omega1.t()*omega2));
+ gamma_denominator = (cross(omega1,omega2).t()*cross(omega1,omega2));
+ gamma = sqrt(gamma_numerator(1,1)/gamma_denominator(1,1));
+
+
+#ifdef DEBUG
+ cout << "SubProblem2::gamma" << endl << gamma << endl << endl;
+#endif
+
+ z = alpha(1,1)*omega1 + beta(1,1)*omega2 + gamma*cross(omega1,omega2);
+ c = z+r;
+
+#ifdef DEBUG
+ PrintMatrix(c,"SubProblem2::c");
+ PrintMatrix(z,"SubProblem2::z");
+#endif
+
+ theta[0] = SubProblem1(c,q,r,omega1);
+ theta[1] = SubProblem1(p,c,r,omega2);
+
+ z = alpha(1,1)*omega1 + beta(1,1)*omega2 - gamma*cross(omega1,omega2);
+ c = z+r;
+
+#ifdef DEBUG
+ PrintMatrix(c,"SubProblem2::c");
+ PrintMatrix(z,"SubProblem2::z");
+#endif
+
+ theta[2] = SubProblem1(c,q,r,omega1);
+ theta[3] = SubProblem1(p,c,r,omega2);
+}
+
Added: pkg/trunk/util/kinematics/libKinematics/src/test/test_PR2ik.cpp
===================================================================
--- pkg/trunk/util/kinematics/libKinematics/src/test/test_PR2ik.cpp (rev 0)
+++ pkg/trunk/util/kinematics/libKinematics/src/test/test_PR2ik.cpp 2008-06-06 00:28:26 UTC (rev 661)
@@ -0,0 +1,46 @@
+#include <libKinematics/kinematics.h>
+#include <libKinematics/ik.h>
+
+using namespace kinematics;
+using namespace PR2;
+using namespace std;
+
+
+int main()
+{
+ NEWMAT::Matrix g(4,4);
+ double theta1 = 0;
+ g = 0;
+
+ PR2Arm myArm;
+
+/* Joint angles and speeds for testing */
+ double angles[7] = {0,0,0,0,0,0,0};
+ double speeds[7] = {0,0,0,0,0,0,0};
+
+
+/* Return test values */
+ NEWMAT::Matrix g0 = myArm.ComputeFK(angles);
+ NEWMAT::Matrix eS = myArm.ComputeEndEffectorVelocity(angles,speeds);
+
+ angles[0] = 0.1;
+ angles[1] = -1;
+ angles[2] = 0.3;
+ angles[3] = 0.3;
+ angles[4] = 0.2;
+ angles[5] = 0.5;
+
+ NEWMAT::Matrix pose = myArm.ComputeFK(angles);
+ g = pose;
+
+ NEWMAT::Matrix theta(7,1);
+ theta = 0;
+ theta = myArm.ComputeIK(g,0.1);
+
+ PrintMatrix(theta,"Answer");
+#ifdef DEBUG
+ cout << "Main::End-effector Pose:" << endl << g << endl ;
+ cout << "Main::End-effector Velocity:" << endl << eS;
+#endif
+}
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
