controllerErrorCode ArmController::setHandCartesianPosLinear(double x, double y, double z, double roll, double pitch, double yaw, double timestep, double stepSize) { double pos; //Set up controller setMode(CONTROLLER_POSITION); this->timeStep = timestep; this->stepSize = stepSize; //Calculate end position KDL::Vector endPos(x,y,z); KDL::Rotation endRot; endRot = endRot.RPY(roll,pitch,yaw); endFrame.p = endPos; endFrame.M = endRot; //Get current location KDL::JntArray q = KDL::JntArray(pr2_kin.nJnts); //Get joint positions for(int i=0;i<ARM_MAX_JOINTS;i++){ armJointControllers[i].getPosAct(&pos); q(i) = pos; } KDL::Frame f; pr2_kin.FK(q,f); startPosition = f.p; //Record Starting location KDL::Vector move = endPos-startPosition; //Vector to ending position double dist = move.Norm(); //Distance to move moveDirection = move/dist; //Normalize movement vector rotInterpolator.SetStartEnd(f.M, endRot); double total_angle = rotInterpolator.Angle(); nSteps = (int)(dist/stepSize); if(nSteps==0){ std::cout<<"ArmController.cpp: Error:: number of steps calculated to be 0"<<std::endl; return CONTROLLER_COMPUTATION_ERROR; } angleStep = total_angle/nSteps; lastTime= getTime(); //Record first time marker stepIndex = 0; //Reset step index linearInterpolation = true; return CONTROLLER_ALL_OK; }
controllerErrorCode ArmController::setHandCartesianPos(double x, double y, double z, double roll, double pitch, double yaw) { linearInterpolation = false; //Define position and rotation KDL::Vector position(x,y,z); KDL::Rotation rotation; rotation = rotation.RPY(roll,pitch,yaw); cmdPos[0] = x; cmdPos[1] = y; cmdPos[2] = z; cmdPos[3] = roll; cmdPos[4] = pitch; cmdPos[5] = yaw; //Create frame based on position and rotation KDL::Frame f; f.p = position; f.M = rotation; return commandCartesianPos(f); }