Example #1
0
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);
    }
}
Example #4
0
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;
}
Example #8
0
// construct vector
urdf::Vector3 toUrdf(const KDL::Vector & v)
{
    return urdf::Vector3(v.x(), v.y(), v.z());
}
Example #9
0
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();
}
Example #11
0
//-----------------------------------------------------------------------------------------------
// 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

}