00001 #ifndef MOTION_PLANNER_H
00002 #define MOTION_PLANNER_H
00003
00004 #include "Math/Vector.h"
00005 #include "InverseKinematics/IKJacobian.h"
00006 #include "InverseKinematics/KinematicsHandler.h"
00007 #include "PathPlanning/PathPlanner.h"
00008 #include "MotionPlannerErrors.h"
00009
00010
00011
00012 enum MPTrajectoryType{
00013 JointInterpolated = 0,
00014 CartesianInterpolated = 1,
00015 JointInterpOrientation = 2
00016 };
00017
00037 class MotionPlanner : public OSCAR::Base
00038 {
00039 public:
00040
00068 MotionPlanner(const OSCAR::DHData& robotData, const OSCAR::Vector& jointOffsets, OSCAR::JointVector& initialJoints, OSCAR::Matrix& jointLimits, OSCAR::Vector& velLimits, OSCAR::Vector& accLimits, OSCAR::Vector& handVelLimits, OSCAR::Vector& handAccLimits, OSCAR::OSCARError &err = OSCAR::DUMMY_ERROR(OSCAR::noError));
00069 ~MotionPlanner();
00070
00086 bool PlanMove(const OSCAR::JointVector currentJoints, const OSCAR::JointVector targetJoints, MPTrajectoryType trajType, double& moveTime);
00087
00103 bool PlanMove(const OSCAR::JointVector currentJoints, const OSCAR::Xform targetHand, MPTrajectoryType trajType, double& moveTime);
00104
00118 bool PlanMoveJogJoint(const OSCAR::JointVector& currentJoints, const OSCAR::JointVector& currentVelocity, const std::vector<float> directions);
00119
00120
00135 bool PlanMoveJogCartesian(const OSCAR::JointVector& currentJoints, const OSCAR::JointVector& currentVelocity, std::vector<double> directions);
00136
00147 bool Stop(const OSCAR::JointVector& currentJoints, const OSCAR::JointVector& currentVelocity, bool fastest=true);
00148
00162 bool GetJointPosition(OSCAR::JointVector& jointPosVector, OSCAR::JointVector& currentVelocity, TrajectoryGenerator::TrajectoryState& state);
00163
00164
00174 bool GetHandPosition(OSCAR::JointVector& joints, OSCAR::Xform& handPosition);
00175
00183 bool GetHandPosition(OSCAR::Xform& handPosition);
00184
00192 bool SetJointPosition(const OSCAR::JointVector& joints){
00193 if (kinPtr){
00194 return kinPtr->UpdateHandState(joints);
00195 }
00196 else
00197 return false;
00198 }
00199
00205 bool SetToolPose(const OSCAR::Xform& toolPose){
00206 if (kinPtr){
00207 return kinPtr->SetTool(toolPose);
00208 }
00209 else
00210 return false;
00211 }
00212
00218 bool SetBasePose(const OSCAR::Xform& basePose){
00219 if (kinPtr){
00220 return kinPtr->SetBase(basePose);
00221 }
00222 else
00223 return false;
00224 }
00225
00232 bool SetCycleRate(float rate){
00233 if (ppPtr){
00234 ppPtr->SetCycleRate(rate);
00235 cycleRate=rate;
00236 }
00237 else{
00238 OSCAR::DisplayError("MotionPlanner::SetCycleRate() - PathPlanner object not properly initialized.");
00239 return false;
00240 }
00241 }
00242
00257 bool SetTrajectoryShape(TrajectoryShapeType shape);
00258
00267 bool SetRampTime(float rampTime);
00268
00276 bool SetCartesianControlMode(OSCAR::CartesianCoordinateMode _coordMode){
00277 coordMode=_coordMode;
00278 return true;
00279 }
00280
00292 bool SetSpeedScale(float scale){
00293 if (!ppPtr->SetTimeScale(scale)){
00294 setError(ppPtr->GetError());
00295 return false;
00296 }
00297 timeScale=scale;
00298 return true;
00299 }
00300
00310 bool SetVelocityScale(float scale);
00311
00321 bool SetAccelerationScale(float scale);
00322
00323 bool ComputeHandVelocity(const OSCAR::JointVector& currentVelocity, OSCAR::HandPose& handVel){
00324 if (!kinPtr->ComputeHandVelocity(currentVelocity,handVel)){
00325 setError(kinPtr->GetError());
00326 return false;
00327 }
00328 return true;
00329 }
00330
00331 protected:
00332 bool checkLimits(const OSCAR::JointVector& joints, bool checkVelocity=true);
00333 double estimateMoveTime(const OSCAR::Vector& start, const OSCAR::Vector& end);
00334 bool getJointPosition(std::vector<OSCAR::Vector>& jointPosBuffer, TrajectoryGenerator::TrajectoryState& state);
00335 bool cancelMotion();
00336
00337 OSCAR::JointVector currentJoints;
00338 OSCAR::JointVector prevJoints;
00339 OSCAR::JointVector prevJoints2;
00340 OSCAR::JointVector currentVelocity;
00341 OSCAR::JointVector targetJoints;
00342 OSCAR::JointVector tempJointPosVector;
00343 OSCAR::JointVector tempJointPosVector2;
00344 std::list<OSCAR::Vector> finalBuffer;
00345
00346 std::string robotName;
00347
00348 TrajectoryGenerator::TrajectoryState prevState;
00349
00350 OSCAR::GeneralKinematicsHandler* kinPtr;
00351 PathPlanner* ppPtr;
00352 unsigned int DOF;
00353
00354 OSCAR::Vector velLimits;
00355 OSCAR::Vector accLimits;
00356 OSCAR::Vector handVelLimits;
00357 OSCAR::Vector handAccLimits;
00358 OSCAR::Vector minVelLimits;
00359 OSCAR::Vector minAccLimits;
00360
00361 TrajectoryShapeType trajShape;
00362 OSCAR::CartesianCoordinateMode coordMode;
00363 double cycleRate;
00364 double velScale;
00365 double accScale;
00366 double rampTime;
00367 double maxLinVel;
00368 double timeScale;
00369 };
00370
00371 #endif
