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