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;
}