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);
     }
   }
 }
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_++;
  }
}