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;
}
TEST_F(VectorExpressionGenerationTest, Constructor)
{
giskard::VectorConstructorSpec descr;
giskard::Scope scope;
descr.set_x(x);
descr.set_y(y);
descr.set_z(z);
KDL::Vector v = descr.get_expression(scope)->value();
EXPECT_DOUBLE_EQ(v.x(), 1.1);
EXPECT_DOUBLE_EQ(v.y(), 2.2);
EXPECT_DOUBLE_EQ(v.z(), 3.3);
}
//Pose is global pose with odometry
void cob_cartesian_trajectories::cartStateCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
if(bRun)
{
ros::Duration dt = ros::Time::now() - timer;
cout << "\n" << "===================================" << "\n\n" << "dt: " << dt << "\n";
timer = ros::Time::now();
if((targetDuration-currentDuration) <= 0)
{
geometry_msgs::Twist twist;
cart_command_pub.publish(twist);
ROS_INFO("finished trajectory in %f", ros::Time::now().toSec() - tstart.toSec());
success = true;
stopTrajectory();
return;
}
KDL::Frame current;
KDL::Frame myhinge;
tf::PoseMsgToKDL(msg->pose, current);
tf::PoseMsgToKDL(current_hinge.pose, myhinge);
KDL::Vector unitz = myhinge.M.UnitZ();
std::cout << "Radius because of Hinge: " << (myhinge.p - current.p) << "UnitZ of hinge: " << unitz.z() << "\n";
// get twist to be published
geometry_msgs::Twist twist;
twist = getTwist(currentDuration, current);
// publish twist
cart_command_pub.publish(twist);
// add needed time
currentDuration+=dt.toSec();
// add position to be visualized in rviz
geometry_msgs::Point p;
p.x = msg->pose.position.x;
p.y = msg->pose.position.y;
p.z = msg->pose.position.z;
trajectory_points.push_back(p);
}
else
{
//publish zero
geometry_msgs::Twist twist;
cart_command_pub.publish(twist);
}
}
int main()
{
KDLCollada kdlCollada;
vector <KDL::Chain> kinematicsModels;
const string filename = "puma.dae"; // loading collada kinematics model and converting it to kdl serial chain
if (!kdlCollada.load(COLLADA_MODELS_PATH + filename, kinematicsModels))
{
cout << "Failed to import " << filename;
return 0;
}
cout << "Imported " << kinematicsModels.size() << " kinematics chains" << endl;
for (unsigned int i = 0; i < kinematicsModels.size(); i++) // parsing output kdl serail chain
{
KDL::Chain chain = kinematicsModels[i];
cout << "Chain " << i << " has " << chain.getNrOfSegments() << " segments" << endl;
for (unsigned int u = 0; u < chain.getNrOfSegments(); u++)
{
KDL::Segment segment = chain.segments[u];
string segmentName = segment.getName();
cout << "Segment " << segmentName << " :" <<endl;
KDL::Frame f_tip = segment.getFrameToTip();
KDL::Vector rotAxis = f_tip.M.GetRot();
double rotAngle = f_tip.M.GetRotAngle(rotAxis);
KDL::Vector trans = f_tip.p;
cout << " frame: rotation " << rotAxis.x() << " " << rotAxis.y() << " " << rotAxis.z() << " " << rotAngle * 180 / M_PI << endl;
cout << " frame: translation " << trans.x() << " " << trans.y() << " " << trans.z() << endl;
KDL::RigidBodyInertia inertia = segment.getInertia();
KDL::Joint joint = segment.getJoint();
string jointName = joint.getName();
string jointType = joint.getTypeName();
KDL::Vector jointAxis = joint.JointAxis();
KDL::Vector jointOrigin = joint.JointOrigin();
cout << " joint name: " << jointName << endl;
cout << " type: " << jointType << endl;
cout << " axis: " << jointAxis.x() << " " <<jointAxis.y() << " " << jointAxis.z() << endl;
cout << " origin: " << jointOrigin.x() << " " << jointOrigin.y() << " " << jointOrigin.z() << endl;
}
}
return 0;
}
// Set vector from tf
void set_vector( tf::StampedTransform t, KDL::Vector& v )
{
v.x(t.getOrigin().x());
v.y(t.getOrigin().y());
v.z(t.getOrigin().z());
}
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;
}
// construct vector
urdf::Vector3 toUrdf(const KDL::Vector & v)
{
return urdf::Vector3(v.x(), v.y(), v.z());
}
double leatherman::distance(const KDL::Vector &a, const KDL::Vector &b)
{
return sqrt((a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()));
}
//KDL vector to eigen
void convert(const KDL::Vector &in, eVector3 &out) {
out<<in.x(), in.y(), in.z();
}
//-----------------------------------------------------------------------------------------------
// FK_ALL
//-----------------------------------------------------------------------------------------------
void LWR4Kinematics::fk_all(const std::vector<double> joint_positions, unsigned int & config, double & arm_angle, std::vector<KDL::Frame> &joint_frames) {
// I modified the implementation of Mirko Kunze here which is written in a generic
// way.
//-----------------------------------------------------------------------------------------------
// Constructing the DH parameters vector
std::vector<std::vector<double> > dh;
dh.clear();
std::vector<double> singlelink;
singlelink.push_back(joint_positions.at(0));
singlelink.push_back(dbs);
singlelink.push_back(0.0);
singlelink.push_back(M_PI / 2);
dh.push_back(singlelink);
singlelink.clear();
singlelink.push_back(joint_positions.at(1));
singlelink.push_back(0.0);
singlelink.push_back(0.0);
singlelink.push_back(-M_PI / 2);
dh.push_back(singlelink);
singlelink.clear();
singlelink.push_back(joint_positions.at(2));
singlelink.push_back(dse);
singlelink.push_back(0.0);
singlelink.push_back(-M_PI / 2);
dh.push_back(singlelink);
singlelink.clear();
singlelink.push_back(joint_positions.at(3));
singlelink.push_back(0.0);
singlelink.push_back(0.0);
singlelink.push_back(M_PI / 2);
dh.push_back(singlelink);
singlelink.clear();
singlelink.push_back(joint_positions.at(4));
singlelink.push_back(dew);
singlelink.push_back(0.0);
singlelink.push_back(M_PI / 2);
dh.push_back(singlelink);
singlelink.clear();
singlelink.push_back(joint_positions.at(5));
singlelink.push_back(0.0);
singlelink.push_back(0.0);
singlelink.push_back(-M_PI / 2);
dh.push_back(singlelink);
singlelink.clear();
singlelink.push_back(joint_positions.at(6));
singlelink.push_back(dwt);
singlelink.push_back(0.0);
singlelink.push_back(0);
dh.push_back(singlelink);
singlelink.clear();
//-----------------------------------------------------------------------------------------------
// Find the configuration parameter
config_fk = ((int) (joint_positions.at(1) < 0)) + (2 * ((int) (joint_positions.at(3) < 0))) + (4 * ((int) (joint_positions.at(5) < 0)));
config = config_fk;
//-----------------------------------------------------------------------------------------------
// Calculate link transformations
for (unsigned int linkIter = 1; linkIter < (n_joints + 1); linkIter++) {
KDL::Vector linkTmpVec = KDL::Vector::Zero();
KDL::Rotation linkTmpRot = KDL::Rotation::Identity();
double cos_theta = cos(dh.at(linkIter - 1).at(0));
double sin_theta = sin(dh.at(linkIter - 1).at(0));
double cos_alpha = cos(dh.at(linkIter - 1).at(3));
double sin_alpha = sin(dh.at(linkIter - 1).at(3));
linkTmpVec = KDL::Vector(
dh.at(linkIter - 1).at(2) * cos_theta,
dh.at(linkIter - 1).at(2) * sin_theta,
dh.at(linkIter - 1).at(1));
linkTmpRot = KDL::Rotation(
cos_theta, -sin_theta * cos_alpha, sin_theta * sin_alpha,
sin_theta, cos_theta * cos_alpha, -cos_theta * sin_alpha,
0.0, sin_alpha, cos_alpha);
joint_frames.at(linkIter) = KDL::Frame(linkTmpRot, linkTmpVec);
joint_frames.at(linkIter) = joint_frames.at(linkIter - 1) * joint_frames.at(linkIter);
}
// taking into consideration the tool transformation
joint_frames.at(n_joints) = joint_frames.at(n_joints) * tr_tool_to_ee;
//-----------------------------------------------------------------------------------------------
// finding the arm angle
KDL::Vector xs = joint_frames.at(1).p;
KDL::Vector xe = joint_frames.at(3).p;
KDL::Vector xw = joint_frames.at(5).p;
KDL::Vector xsw = xw - xs;
KDL::Vector xse = xe - xs;
KDL::Vector usin = KDL::Vector(0.0, 0.0, 1.0) * xsw;
usin.Normalize();
KDL::Vector ucos = xsw * usin;
ucos.Normalize();
// Nima: added negative sign so that the rotation of arm_angle is around the vector from shoulder to wrist.
arm_angle = -((double) atan2(dot(xse, usin), dot(xse, ucos)));
/// TOTAL TIME MEASURED FROM OUTSIDE ~= 9 uS
}
