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