00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #include "CartesianVelocityController.hpp"
00021 #include <kdl/kinfam_io.hpp>
00022 #include <kdl/chainiksolvervel_pinv_givens.hpp>
00023 #include <kdl/chainiksolvervel_pinv.hpp>
00024 #include <kdl/chainfksolverpos_recursive.hpp>
00025
00026 #include <ocl/ComponentLoader.hpp>
00027 ORO_LIST_COMPONENT_TYPE( OCL::CartesianVelocityController );
00028
00029 namespace OCL
00030 {
00031 using namespace std;
00032 using namespace RTT;
00033 using namespace KDL;
00034
00035 CartesianVelocityController::CartesianVelocityController(string name) :
00036 TaskContext(name,PreOperational),
00037 cartpos_port("CartesianSensorPosition",KDL::Frame::Identity()),
00038 cartvel_port("CartesianOutputVelocity",KDL::Twist::Zero()),
00039 naxespos_port("nAxesSensorPosition"),
00040 naxesvel_port("nAxesOutputVelocity"),
00041 chain_prop("Chain","Kinematic Description of the robot chain"),
00042 toolframe("ToolLocation","Offset between the robot's end effector and the tool location"),
00043 kinematics_status(true)
00044 {
00045
00046 this->ports()->addPort(&cartpos_port);
00047 this->ports()->addPort(&cartvel_port);
00048 this->ports()->addPort(&naxespos_port);
00049 this->ports()->addPort(&naxesvel_port);
00050
00051 this->properties()->addProperty(&chain_prop);
00052 this->properties()->addProperty(&toolframe);
00053
00054 }
00055
00056 CartesianVelocityController::~CartesianVelocityController()
00057 {
00058 }
00059
00060 bool CartesianVelocityController::configureHook()
00061 {
00062 chain = chain_prop;
00063 chain.addSegment(Segment(Joint(Joint::None),toolframe.value()));
00064
00065 nj = chain.getNrOfJoints();
00066 jointpositions=new JntArray(nj);
00067 jointvelocities=new JntArray(nj);
00068 naxesposition.resize(nj);
00069 naxesvelocities.resize(nj);
00070
00071 fksolver=new ChainFkSolverPos_recursive(chain);
00072 iksolver=new ChainIkSolverVel_pinv(chain);
00073 return true;
00074
00075 }
00076
00077 void CartesianVelocityController::cleanupHook()
00078 {
00079 delete jointpositions;
00080 delete jointvelocities;
00081 delete fksolver;
00082 delete iksolver;
00083 }
00084
00085 bool CartesianVelocityController::startHook()
00086 {
00087
00088 this->updateHook();
00089
00090 return kinematics_status>=0;
00091 }
00092
00093 void CartesianVelocityController::updateHook()
00094 {
00095
00096 naxespos_port.Get(naxesposition);
00097 cartvel_port.Get(cartvel);
00098
00099
00100 if(nj==naxesposition.size()){
00101
00102
00103 for(unsigned int i=0;i<nj;i++)
00104 (*jointpositions)(i)=naxesposition[i];
00105
00106
00107 kinematics_status = fksolver->JntToCart(*jointpositions,cartpos);
00108
00109 if(kinematics_status>=0)
00110 cartpos_port.Set(cartpos);
00111 else
00112 log(Error)<<"Could not calculate forward kinematics"<<endlog();
00113
00114
00115 kinematics_status = iksolver->CartToJnt(*jointpositions,cartvel,*jointvelocities);
00116 if(kinematics_status<0){
00117 SetToZero(*jointvelocities);
00118 log(Error)<<"Could not calculate inverse kinematics"<<endlog();
00119 }
00120
00121 for(unsigned int i=0;i<nj;i++)
00122 naxesvelocities[i]=(*jointvelocities)(i);
00123
00124 naxesvel_port.Set(naxesvelocities);
00125 }
00126 else
00127 kinematics_status=-1;
00128 }
00129
00130 void CartesianVelocityController::stopHook()
00131 {
00132 for(unsigned int i=0;i<nj;i++)
00133 naxesvelocities[i]=0.0;
00134 naxesvel_port.Set(naxesvelocities);
00135 }
00136
00137 }
00138
00139
00140
00141
00142