Exemple #1
0
 void convertKDLJacToVispMat(const KDL::Jacobian &kdlJac,
                             vpMatrix &vispMat)
 {
   for(unsigned int i=0; i<kdlJac.rows(); ++i)
   {
     for(unsigned int j=0; j<kdlJac.columns(); ++j)
     {
       vispMat[i][j] = kdlJac.data(i,j);
     }
   }
 }
Exemple #2
0
  vpMatrix kdlJacobianToVispJacobian(const KDL::Jacobian& kdlJacobian,
                                     const vpHomogeneousMatrix& bMe)
  {
    vpMatrix vpJacobian(kdlJacobian.rows(), kdlJacobian.columns());

    KDL::Jacobian kdlJacobianCopy = kdlJacobian;

    // Fix reference point so that it is in the base frame
    vpTranslationVector bOe; //origin of end frame in base frame
    bMe.extract(bOe);
    kdlJacobianCopy.changeRefPoint(KDL::Vector(-bOe[0], -bOe[1], -bOe[2]));
    pal::convertKDLJacToVispMat(kdlJacobianCopy, vpJacobian);// KDL data --> Visp data
    return vpJacobian;
  }
// -------------------------------------------------------------------------------------
KDL::Jacobian JacoIKSolver::desiredJacobian(Eigen::MatrixXd qd)
{
	KDL::JntArray joint_in;
	KDL::Jacobian jac;
	joint_in.resize(6);
	jac.resize(chain_.getNrOfJoints());
	
	qd = DTR*qd;
	
	for (int i = 0; i < 6; i++){
		joint_in(i) = qd(0,i);
	}
	
	jnt_to_jac_solver_->JntToJac(joint_in, jac);
	
	return jac;
}
KDL::Jacobian JacoIKSolver::jointToJacobian(JacoAngles JAngle)
{
	KDL::JntArray joint_in;
	KDL::Jacobian jac;
	joint_in.resize(6);
	jac.resize(chain_.getNrOfJoints());
			
	joint_in(0) = ( JAngle.Actuator1 - 180.0 ) * DTR;
	joint_in(1) = ( JAngle.Actuator2 - 270.0 ) * DTR;
	joint_in(2) = ( JAngle.Actuator3 - 90.0  ) * DTR;
	joint_in(3) = ( JAngle.Actuator4 - 180.0 ) * DTR;
	joint_in(4) = ( JAngle.Actuator5 - 180.0 ) * DTR;
	joint_in(5) = ( JAngle.Actuator6 - 270.0 ) * DTR;
	
	jnt_to_jac_solver_->JntToJac(joint_in, jac);
	
	return jac;
}
    void ITRCartesianImpedanceController::multiplyJacobian(const KDL::Jacobian& jac, const KDL::Wrench& src, KDL::JntArray& dest)
    {
        Eigen::Matrix<double,6,1> w;
        w(0) = src.force(0);
        w(1) = src.force(1);
        w(2) = src.force(2);
        w(3) = src.torque(0);
        w(4) = src.torque(1);
        w(5) = src.torque(2);

        Eigen::MatrixXd j(jac.rows(), jac.columns());
        j = jac.data;
        j.transposeInPlace();

        Eigen::VectorXd t(jac.columns());
        t = j*w;

        dest.resize(jac.columns());
        for (unsigned i=0; i<jac.columns(); i++)
        dest(i) = t(i);
    }
    VirtualForcePublisher()
    {
        ros::NodeHandle n_tilde("~");
        ros::NodeHandle n;

        // subscribe to joint state
        joint_state_sub_ = n.subscribe("joint_states", 1, &VirtualForcePublisher::callbackJointState, this);
        wrench_stamped_pub_ = n.advertise<geometry_msgs::WrenchStamped>("wrench", 1);

        // set publish frequency
        double publish_freq;
        n_tilde.param("publish_frequency", publish_freq, 50.0);
        publish_interval_ = ros::Duration(1.0/std::max(publish_freq,1.0));

        //set time constant of low pass filter
        n_tilde.param("time_constant", t_const_, 0.3);
        // set root and tip
        n_tilde.param("root", root, std::string("torso_lift_link"));
        n_tilde.param("tip", tip, std::string("l_gripper_tool_frame"));

        // load robot model
        urdf::Model robot_model;
        robot_model.initParam("robot_description");

        KDL::Tree kdl_tree;
        kdl_parser::treeFromUrdfModel(robot_model, kdl_tree);
        kdl_tree.getChain(root, tip, chain_);
        jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(chain_));

        jnt_pos_.resize(chain_.getNrOfJoints());
        jacobian_.resize(chain_.getNrOfJoints());

        for (size_t i=0; i<chain_.getNrOfSegments(); i++) {
            if (chain_.getSegment(i).getJoint().getType() != KDL::Joint::None) {
                std::cerr << "kdl_chain(" << i << ") " << chain_.getSegment(i).getJoint().getName().c_str() << std::endl;
            }
        }
    }
void CartTrajInterp::updateHook(){
  
  // Read the ports
  RTT::FlowStatus ftraj = this->port_traj_in_.read(this->traj_in_);
  RTT::FlowStatus fp = this->port_joint_position_in_.read(this->joint_position_in_);
  RTT::FlowStatus fv = this->port_joint_velocity_in_.read(this->joint_velocity_in_);
  
  // If we get a new trajectory update it and start with point 0
  if(ftraj == RTT::NewData)
  {
    RTT::log(RTT::Info) << "New trajectory with "<<traj_in_.result.planned_trajectory.joint_trajectory.points.size() <<" points" << RTT::endlog();
    if(!this->interpolate(traj_in_.result.planned_trajectory.joint_trajectory, traj_curr_)){
      RTT::log(RTT::Error) << "Interpolation of the trajectory failed" << RTT::endlog();
      return;
    }
    RTT::log(RTT::Info) << "Interpolated traj with "<<traj_curr_.points.size() <<" points" << RTT::endlog();
    this->traj_pt_nb_ = 0;
  }

  // Return if not giving any robot update (might happend during startup)
  if(fp == RTT::NoData || fv == RTT::NoData)
    return
  
  // Update the chain model
  this->arm_.setState(this->joint_position_in_,this->joint_velocity_in_);
  this->arm_.updateModel();
  
  // Check trajectory is not empty
  if (this->traj_curr_.points.size() < 1)
    return;
  
  // Check trajectory is not finished
  if (this->traj_pt_nb_ > this->traj_curr_.points.size() - 1)
    return;
  
  // Pick current point in trajectory     
  for(unsigned int i = 0; i < this->dof_; ++i){
    this->pt_in_.q(i) = this->traj_curr_.points[this->traj_pt_nb_].positions[i];
    this->pt_in_.qdot(i) = this->traj_curr_.points[this->traj_pt_nb_].velocities[i] ;
    this->pt_in_.qdotdot(i) = this->traj_curr_.points[this->traj_pt_nb_].accelerations[i] ;
  }
  
  // Compute FK
  KDL::FrameVel traj_pt_vel;
  KDL::JntArrayVel pt_in_vel;
  pt_in_vel.q = this->pt_in_.q;
  pt_in_vel.qdot = this->pt_in_.qdot;
  KDL::Twist JdotQdot;
  
  // If FK or JdotQdot solver fails start over 
  if ((this->fk_solver_vel_->JntToCart(pt_in_vel, traj_pt_vel) == 0) &&  (this->jntToJacDotSolver_->JntToJacDot(pt_in_vel, JdotQdot) == 0) ){
        
    KDL::Jacobian J = this->arm_.getJacobian();
    KDL::Twist xdotdot;
    
    // Xdd = Jd * qd + J * qdd
    for(unsigned int i = 0; i < 6; ++i )
      xdotdot(i) = JdotQdot(i) + J.data(i) * this->pt_in_.qdotdot(i);
    
    this->traj_pt_out_ = KDL::FrameAcc(traj_pt_vel.GetFrame(), traj_pt_vel.GetTwist(), xdotdot);
    
    // Send cartesian trajectory point to the controller
    this->port_traj_pt_out_.write(this->traj_pt_out_);
    this->port_traj_joint_out_.write(this->pt_in_);

    // Increment counter
    this->traj_pt_nb_++;
  }
}
int SNS_IK::CartToJntVel(const KDL::JntArray& q_in, const KDL::Twist& v_in,
                         const KDL::JntArray& q_bias,
                         const std::vector<std::string>& biasNames,
                         const KDL::JntArray& q_vel_bias,
                         KDL::JntArray& qdot_out)
{
  if (!m_initialized) {
    ROS_ERROR("SNS_IK was not properly initialized with a valid chain or limits.");
    return -1;
  }

  KDL::Jacobian jacobian;
  jacobian.resize(q_in.rows());
  if (m_jacobianSolver->JntToJac(q_in, jacobian) < 0)
  {
    std::cout << "JntToJac failed" << std::endl;
    return -1;
  }

  std::vector<Task> sot;
  Task task;
  task.jacobian = jacobian.data;
  task.desired = VectorD::Zero(6);
  // twistEigenToKDL
  for(size_t i = 0; i < 6; i++)
      task.desired(i) = v_in[i];
  sot.push_back(task);

  // Calculate the nullspace goal as a configuration-space task.
  // Creates a task Jacobian which maps the provided nullspace joints to
  // the full joint state.
  if (q_bias.rows()) {
    Task task2;
    std::vector<int> indicies;
    if (!nullspaceBiasTask(q_bias, biasNames, &(task2.jacobian), &indicies)) {
      ROS_ERROR("Could not create nullspace bias task");
      return -1;
    }
    task2.desired = VectorD::Zero(q_bias.rows());
    for (size_t ii = 0; ii < q_bias.rows(); ++ii) {
      // This calculates a "nullspace velocity".
      // There is an arbitrary scale factor which will be set by the max scale factor.
      task2.desired(ii) = m_nullspaceGain * (q_bias(ii) - q_in(indicies[ii])) / m_loopPeriod;
      // TODO: may want to limit the NS velocity to 70-90% of max joint velocity
    }
    sot.push_back(task2);
  }

  // Bias the joint velocities
  // If the bias is the previous joint velocities, this is velocity damping
  if(q_vel_bias.rows() == q_in.rows()) {
    Task task2;
    task2.jacobian = MatrixD::Identity(q_vel_bias.rows(), q_vel_bias.rows());
    task2.desired = VectorD::Zero(q_vel_bias.rows());
    for (size_t ii = 0; ii < q_vel_bias.rows(); ++ii) {
      task2.desired(ii) = q_vel_bias(ii);
    }
    sot.push_back(task2);
  }

  return m_ik_vel_solver->getJointVelocity(&qdot_out.data, sot, q_in.data);
}
    void callbackJointState(const JointStateConstPtr& state)
    {
        std::map<std::string, double> joint_name_position;
        if (state->name.size() != state->position.size()) {
            ROS_ERROR("Robot state publisher received an invalid joint state vector");
            return;
        }

        // determine least recently published joint
        ros::Time last_published = ros::Time::now();
        for (unsigned int i=0; i<state->name.size(); i++) {
            ros::Time t = last_publish_time_[state->name[i]];
            last_published = (t < last_published) ? t : last_published;
        }

        if (state->header.stamp >= last_published + publish_interval_) {
            // get joint positions from state message
            std::map<std::string, double> joint_positions;
            std::map<std::string, double> joint_efforts;
            for (unsigned int i=0; i<state->name.size(); i++) {
                joint_positions.insert(make_pair(state->name[i], state->position[i]));
                joint_efforts.insert(make_pair(state->name[i], state->effort[i]));
            }

            KDL::JntArray  tau;
            KDL::Wrench    F;
            Eigen::Matrix<double,Eigen::Dynamic,6> jac_t;
            Eigen::Matrix<double,6,Eigen::Dynamic> jac_t_pseudo_inv;
            tf::StampedTransform transform;
            KDL::Wrench    F_pub;
            tf::Vector3 tf_force;
            tf::Vector3 tf_torque;

            tau.resize(chain_.getNrOfJoints());

            //getPositions;
            for (size_t i=0, j=0; i<chain_.getNrOfSegments(); i++) {
                if (chain_.getSegment(i).getJoint().getType() == KDL::Joint::None)
                    continue;

                // check
                std::string joint_name = chain_.getSegment(i).getJoint().getName();
                if ( joint_positions.find(joint_name) == joint_positions.end() ) {
                    ROS_ERROR("Joint '%s' is not found in joint state vector", joint_name.c_str());
                }

                // set position
                jnt_pos_(j) = joint_positions[joint_name];
                tau(j) = joint_efforts[joint_name];
                j++;
            }

            jnt_to_jac_solver_->JntToJac(jnt_pos_, jacobian_);
            jac_t = jacobian_.data.transpose();
            if ( jacobian_.columns() >= jacobian_.rows() ) {
                jac_t_pseudo_inv =(jac_t.transpose() * jac_t).inverse() *  jac_t.transpose();
            } else {
                jac_t_pseudo_inv =jac_t.transpose() * ( jac_t *  jac_t.transpose() ).inverse();
            }
#if 1
            {
                ROS_INFO("jac_t# jac_t : ");
                Eigen::Matrix<double,6,6> mat_i =  mat_i = jac_t_pseudo_inv * jac_t;
                for (unsigned int i = 0; i < 6; i++) {
                    std::stringstream ss;
                    for (unsigned int j=0; j<6; j++)
                        ss << std::fixed << std::setw(8) << std::setprecision(4) << mat_i(j,i) << " ";
                    ROS_INFO_STREAM(ss.str());
                }
            }
#endif
            // f = - inv(jt) * effort
            for (unsigned int j=0; j<6; j++)
            {
                F(j) = 0;
                for (unsigned int i = 0; i < jnt_pos_.rows(); i++)
                {
                    F(j) += -1 * jac_t_pseudo_inv(j, i) * tau(i);
                }
            }

            //low pass filter
            F += (F_pre_ - F) * exp(-1.0 / t_const_);
            for (unsigned int j=0; j<6; j++) {
                F_pre_(j) = 0;
            }
            F_pre_ += F;

            //tf transformation
            tf::vectorKDLToTF(F.force, tf_force);
            tf::vectorKDLToTF(F.torque, tf_torque);
            try {
                listener_.waitForTransform( tip, root, state->header.stamp, ros::Duration(1.0));
                listener_.lookupTransform( tip, root, state->header.stamp , transform);
            }
            catch (tf::TransformException ex) {
                ROS_ERROR("%s",ex.what());
                ros::Duration(1.0).sleep();
            }
            for (unsigned int j=0; j<3; j++) {
                F_pub.force[j] = transform.getBasis()[j].dot(tf_force);
                F_pub.torque[j] = transform.getBasis()[j].dot(tf_torque);
            }

            geometry_msgs::WrenchStamped msg;
            msg.header.stamp = state->header.stamp;
            msg.header.frame_id = tip;
            // http://code.ros.org/lurker/message/20110107.151127.7177af06.nl.html
            //tf::WrenchKDLToMsg(F,msg.wrench);
            msg.wrench.force.x = F_pub.force[0];
            msg.wrench.force.y = F_pub.force[1];
            msg.wrench.force.z = F_pub.force[2];
            msg.wrench.torque.x = F_pub.torque[0];
            msg.wrench.torque.y = F_pub.torque[1];
            msg.wrench.torque.z = F_pub.torque[2];
            wrench_stamped_pub_.publish(msg);

            {
                ROS_INFO("jacobian : ");
                for (unsigned int i = 0; i < jnt_pos_.rows(); i++) {
                    std::stringstream ss;
                    for (unsigned int j=0; j<6; j++)
                        ss << std::fixed << std::setw(8) << std::setprecision(4) << jacobian_(j,i) << " ";
                    ROS_INFO_STREAM(ss.str());
                }
                ROS_INFO("effort : ");
                std::stringstream sstau;
                for (unsigned int i = 0; i < tau.rows(); i++) {
                    sstau << std::fixed << std::setw(8) << std::setprecision(4) << tau(i) << " ";
                }
                ROS_INFO_STREAM(sstau.str());
                ROS_INFO("force : ");
                std::stringstream ssf;
                for (unsigned int j = 0; j < 6; j++) {
                    ssf << std::fixed << std::setw(8) << std::setprecision(4) << F(j) << " ";
                }
                ROS_INFO_STREAM(ssf.str());
            }

            // store publish time in joint map
            for (unsigned int i=0; i<state->name.size(); i++)
                last_publish_time_[state->name[i]] = state->header.stamp;
        }
    }
Exemple #10
0
 vpMatrix convertKDLJacToVispMat(const KDL::Jacobian& kdlJac)
 {
   vpMatrix vispMat(kdlJac.rows(), kdlJac.columns());
   convertKDLJacToVispMat(kdlJac, vispMat);
   return vispMat;
 }
int main(int argc, char *argv[]) 
{
    ros::init(argc, argv, "kinematics_publisher");
    ros::NodeHandle nh;
    ros::Publisher joint_publisher;
    ros::Publisher frame_publisher;

    joint_publisher = nh.advertise<sensor_msgs::JointState>("kinematics/joint_states", 1, true);
    frame_publisher = nh.advertise<geometry_msgs::PoseStamped>("/frame_result", 1, true);

    std::vector<std::string> names;
    names.resize(2);
    names.at(0) = "base_to_body";
    names.at(1) = "body_to_head";

    sensor_msgs::JointState my_joints;
    my_joints.name = names;
    my_joints.position.resize(2);
    my_joints.position.at(0) =  0.2;
    my_joints.position.at(1) = 0,2;

    joint_publisher.publish(my_joints);

    ros::spinOnce();
    std::cin.get();
	bool exit_value;
	KDL::Tree my_tree;
	urdf::Model my_model;
	

    if (!kdl_parser::treeFromParam("/robot_description", my_tree)){
        ROS_ERROR("Failed to construct kdl tree");
        return false;
    }


    std::cout << "Number of links of the urdf:" << my_tree.getNrOfSegments() << std::endl;
    std::cout << "Number of Joints of the urdf:" << my_tree.getNrOfJoints() << std::endl;

    KDL::Chain my_chain;
    my_tree.getChain("world", "webcam", my_chain);

    std::cout << "Number of links of the CHAIN:" << my_chain.getNrOfSegments() << std::endl;
    std::cout << "Number of Joints of the CHAIN:" << my_chain.getNrOfJoints() << std::endl;


    KDL::ChainFkSolverPos_recursive fksolver(my_chain);
    KDL::JntArray q(my_chain.getNrOfJoints());
    q(0) =  my_joints.position.at(0);
    q(1) =  my_joints.position.at(1);

    KDL::Frame webcam;
    fksolver.JntToCart(q,webcam);

    geometry_msgs::Pose webcam_pose;
    tf::poseKDLToMsg(webcam, webcam_pose);

    geometry_msgs::PoseStamped webcam_stamped;
    webcam_stamped.pose = webcam_pose;
    webcam_stamped.header.frame_id = "world";
    webcam_stamped.header.stamp = ros::Time::now();

    frame_publisher.publish(webcam_stamped);
    ros::spinOnce();

    KDL::ChainJntToJacSolver jacobian_solver(my_chain);
    KDL::Jacobian J;
    J.resize(2);
    jacobian_solver.JntToJac(q, J);

    std::cout << J.data << std::endl;

    webcam.M.DoRotY(-20*KDL::deg2rad);

    tf::poseKDLToMsg(webcam, webcam_pose);
    webcam_stamped.pose = webcam_pose;
    webcam_stamped.header.stamp = ros::Time::now();

    frame_publisher.publish(webcam_stamped);
    ros::spinOnce();

    std::cin.get();

    KDL::ChainIkSolverVel_pinv ik_solver_vel(my_chain);
    KDL::ChainIkSolverPos_NR ik_solver_pos(my_chain,fksolver,ik_solver_vel);
    KDL::JntArray q_sol(2);

    ik_solver_pos.CartToJnt(q, webcam,q_sol);

    my_joints.position.at(0) =  q_sol(0);
    my_joints.position.at(1) = q_sol(1);

    joint_publisher.publish(my_joints);

    ros::spinOnce();
    std::cin.get();

    KDL::Vector g (0.0,0.0,-9.81);
    KDL::ChainDynParam my_chain_params(my_chain, g);
    KDL::ChainIdSolver_RNE id_solver(my_chain, g);

    KDL::JntArray G(2);
    my_chain_params.JntToGravity(q_sol,G);

    KDL::JntSpaceInertiaMatrix H(2);
    my_chain_params.JntToMass(q_sol, H);

    KDL::JntArray C(2);
    KDL::JntArray q_dot(2);
    q_dot(0) = 0.02;
    q_dot(1) = 0.04;
    my_chain_params.JntToCoriolis(q_sol,q_dot, C);

    std::cout << "g(q): " <<G.data << std::endl;
    std::cout << "M(q): " <<H.data << std::endl;
    std::cout << "C(q,q_dot): " <<C.data << std::endl;


    KDL::JntArray q_dotdot(2);
    q_dotdot(0) = -0.02;
    q_dotdot(1) = 0.02;
    KDL::Wrenches f(4);
    KDL::JntArray tau(2);
    id_solver.CartToJnt(q_sol, q_dot, q_dotdot, f, tau);
    std::cout << "Tau: " <<tau.data << std::endl;

    std::cin.get();
    moveit::planning_interface::MoveGroup my_group("webcam_robot");
    my_group.setNamedTarget("look_left");
    my_group.move();


    return false;
}