void seqplay::setJointAngle(unsigned int i_rank, double jv, double tm)
{
double pos[m_dof];
getJointAngles(pos);
pos[i_rank] = jv;
interpolators[Q]->setGoal(pos, tm);
}
void jointPublish(ros::Publisher jP) {
updateMotor();
/* ************** Publish joint angles ************** */
sensor_msgs::JointState msg;// = boost::make_shared<sensor_msgs::JointState>();
// msg.reset(new sensor_msgs::JointState);
std::vector<std::string> joint_names;
std::vector<float> angles;
std::vector<float> vels;
getJointNames(joint_names);
getJointAngles(angles);
getJointVelocities(vels);
for (int i = 0; i < 2 ; ++i) {
msg.name.push_back(joint_names[i]);
msg.position.push_back(angles[i]);
msg.velocity.push_back(vels[i]);
}
msg.header.stamp = ros::Time::now();
jP.publish(msg);
}
