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; }
void UrdfModelMarker::setJointAngle(boost::shared_ptr<const Link> link, double joint_angle){ string link_frame_name_ = tf_prefix_ + link->name; boost::shared_ptr<Joint> parent_joint = link->parent_joint; if(parent_joint == NULL){ return; } KDL::Frame initialFrame; KDL::Frame presentFrame; KDL::Rotation rot; KDL::Vector rotVec; KDL::Vector jointVec; std_msgs::Header link_header; int rotation_count = 0; switch(parent_joint->type){ case Joint::REVOLUTE: case Joint::CONTINUOUS: { if(joint_angle > M_PI){ rotation_count = (int)((joint_angle + M_PI) / (M_PI * 2)); joint_angle -= rotation_count * M_PI * 2; }else if(joint_angle < -M_PI){ rotation_count = (int)((- joint_angle + M_PI) / (M_PI * 2)); joint_angle -= rotation_count * M_PI * 2; } linkProperty *link_property = &linkMarkerMap[link_frame_name_]; link_property->joint_angle = joint_angle; link_property->rotation_count = rotation_count; tf::poseMsgToKDL (link_property->initial_pose, initialFrame); tf::poseMsgToKDL (link_property->initial_pose, presentFrame); jointVec = KDL::Vector(link_property->joint_axis.x, link_property->joint_axis.y, link_property->joint_axis.z); presentFrame.M = KDL::Rotation::Rot(jointVec, joint_angle) * initialFrame.M; tf::poseKDLToMsg(presentFrame, link_property->pose); break; } case Joint::PRISMATIC: { linkProperty *link_property = &linkMarkerMap[link_frame_name_]; link_property->joint_angle = joint_angle; link_property->rotation_count = rotation_count; tf::poseMsgToKDL (link_property->initial_pose, initialFrame); tf::poseMsgToKDL (link_property->initial_pose, presentFrame); jointVec = KDL::Vector(link_property->joint_axis.x, link_property->joint_axis.y, link_property->joint_axis.z); jointVec = jointVec / jointVec.Norm(); // normalize vector presentFrame.p = joint_angle * jointVec + initialFrame.p; tf::poseKDLToMsg(presentFrame, link_property->pose); break; } default: break; } link_header.stamp = ros::Time(0); link_header.frame_id = linkMarkerMap[link_frame_name_].frame_id; server_->setPose(link_frame_name_, linkMarkerMap[link_frame_name_].pose, link_header); //server_->applyChanges(); callSetDynamicTf(linkMarkerMap[link_frame_name_].frame_id, link_frame_name_, Pose2Transform(linkMarkerMap[link_frame_name_].pose)); }
void UrdfModelMarker::getJointState(boost::shared_ptr<const Link> link) { string link_frame_name_ = tf_prefix_ + link->name; boost::shared_ptr<Joint> parent_joint = link->parent_joint; if(parent_joint != NULL){ KDL::Frame initialFrame; KDL::Frame presentFrame; KDL::Rotation rot; KDL::Vector rotVec; KDL::Vector jointVec; double jointAngle; double jointAngleAllRange; switch(parent_joint->type){ case Joint::REVOLUTE: case Joint::CONTINUOUS: { linkProperty *link_property = &linkMarkerMap[link_frame_name_]; tf::poseMsgToKDL (link_property->initial_pose, initialFrame); tf::poseMsgToKDL (link_property->pose, presentFrame); rot = initialFrame.M.Inverse() * presentFrame.M; jointAngle = rot.GetRotAngle(rotVec); jointVec = KDL::Vector(link_property->joint_axis.x, link_property->joint_axis.y, link_property->joint_axis.z); if( KDL::dot(rotVec,jointVec) < 0){ jointAngle = - jointAngle; } if(link_property->joint_angle > M_PI/2 && jointAngle < -M_PI/2){ link_property->rotation_count += 1; }else if(link_property->joint_angle < -M_PI/2 && jointAngle > M_PI/2){ link_property->rotation_count -= 1; } link_property->joint_angle = jointAngle; jointAngleAllRange = jointAngle + link_property->rotation_count * M_PI * 2; if(parent_joint->type == Joint::REVOLUTE && parent_joint->limits != NULL){ bool changeMarkerAngle = false; if(jointAngleAllRange < parent_joint->limits->lower){ jointAngleAllRange = parent_joint->limits->lower + 0.001; changeMarkerAngle = true; } if(jointAngleAllRange > parent_joint->limits->upper){ jointAngleAllRange = parent_joint->limits->upper - 0.001; changeMarkerAngle = true; } if(changeMarkerAngle){ setJointAngle(link, jointAngleAllRange); } } joint_state_.position.push_back(jointAngleAllRange); joint_state_.name.push_back(parent_joint->name); break; } case Joint::PRISMATIC: { KDL::Vector pos; linkProperty *link_property = &linkMarkerMap[link_frame_name_]; tf::poseMsgToKDL (link_property->initial_pose, initialFrame); tf::poseMsgToKDL (link_property->pose, presentFrame); pos = presentFrame.p - initialFrame.p; jointVec = KDL::Vector(link_property->joint_axis.x, link_property->joint_axis.y, link_property->joint_axis.z); jointVec = jointVec / jointVec.Norm(); // normalize vector jointAngle = KDL::dot(jointVec, pos); link_property->joint_angle = jointAngle; jointAngleAllRange = jointAngle; if(parent_joint->type == Joint::PRISMATIC && parent_joint->limits != NULL){ bool changeMarkerAngle = false; if(jointAngleAllRange < parent_joint->limits->lower){ jointAngleAllRange = parent_joint->limits->lower + 0.003; changeMarkerAngle = true; } if(jointAngleAllRange > parent_joint->limits->upper){ jointAngleAllRange = parent_joint->limits->upper - 0.003; changeMarkerAngle = true; } if(changeMarkerAngle){ setJointAngle(link, jointAngleAllRange); } } joint_state_.position.push_back(jointAngleAllRange); joint_state_.name.push_back(parent_joint->name); break; } case Joint::FIXED: break; default: break; } server_->applyChanges(); } for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++){ getJointState(*child); } return; }