void HrpsysSeqStateROSBridge::onJointTrajectory(trajectory_msgs::JointTrajectory trajectory) {
m_mutex.lock();
ROS_INFO_STREAM("[" << getInstanceName() << "] @onJointTrajectoryAction ");
//std::cerr << goal->trajectory.joint_names.size() << std::endl;
OpenHRP::dSequenceSequence angles;
OpenHRP::dSequence duration;
angles.length(trajectory.points.size()) ;
duration.length(trajectory.points.size()) ;
std::vector<std::string> joint_names = trajectory.joint_names;
for (unsigned int i=0; i < trajectory.points.size(); i++) {
angles[i].length(body->joints().size());
trajectory_msgs::JointTrajectoryPoint point = trajectory.points[i];
for (unsigned int j=0; j < trajectory.joint_names.size(); j++ ) {
body->link(joint_names[j])->q = point.positions[j];
}
body->calcForwardKinematics();
int j = 0;
std::vector<hrp::Link*>::const_iterator it = body->joints().begin();
while ( it != body->joints().end() ) {
hrp::Link* l = ((hrp::Link*)*it);
angles[i][j] = l->q;
++it;
++j;
}
ROS_INFO_STREAM("[" << getInstanceName() << "] @onJointTrajectoryAction : time_from_start " << trajectory.points[i].time_from_start.toSec());
if ( i > 0 ) {
duration[i] = trajectory.points[i].time_from_start.toSec() - trajectory.points[i-1].time_from_start.toSec();
} else { // if i == 0
if ( trajectory.points.size()== 1 ) {
duration[i] = trajectory.points[i].time_from_start.toSec();
} else { // 0.2 is magic number writtein in roseus/euslisp/robot-interface.l
duration[i] = trajectory.points[i].time_from_start.toSec() - 0.2;
}
}
}
m_mutex.unlock();
if ( duration.length() == 1 ) {
m_service0->setJointAngles(angles[0], duration[0]);
} else {
OpenHRP::dSequenceSequence rpy, zmp;
m_service0->playPattern(angles, rpy, zmp, duration);
}
interpolationp = true;
}
bool SequencePlayer::setJointAnglesSequenceOfGroup(const char *gname, const OpenHRP::dSequenceSequence angless, const OpenHRP::dSequence& times)
{
if ( m_debugLevel > 0 ) {
std::cerr << __PRETTY_FUNCTION__ << std::endl;
}
Guard guard(m_mutex);
if (!setInitialState()) return false;
if (!m_seq->resetJointGroup(gname, m_qInit.data.get_buffer())) return false;
std::vector<const double*> v_poss;
std::vector<double> v_tms;
for ( unsigned int i = 0; i < angless.length(); i++ ) v_poss.push_back(angless[i].get_buffer());
for ( unsigned int i = 0; i < times.length(); i++ ) v_tms.push_back(times[i]);
return m_seq->setJointAnglesSequenceOfGroup(gname, v_poss, v_tms, angless.length()>0?angless[0].length():0);
}
bool SequencePlayer::setJointAnglesSequence(const OpenHRP::dSequenceSequence angless, const OpenHRP::bSequence& mask, const OpenHRP::dSequence& times)
{
if ( m_debugLevel > 0 ) {
std::cerr << __PRETTY_FUNCTION__ << std::endl;
}
Guard guard(m_mutex);
if (!setInitialState()) return false;
bool tmp_mask[robot()->numJoints()];
if (mask.length() != robot()->numJoints()) {
for (unsigned int i=0; i < robot()->numJoints(); i++) tmp_mask[i] = true;
}else{
for (unsigned int i=0; i < robot()->numJoints(); i++) tmp_mask[i] = mask.get_buffer()[i];
}
int len = angless.length();
std::vector<const double*> v_poss;
std::vector<double> v_tms;
for ( unsigned int i = 0; i < angless.length(); i++ ) v_poss.push_back(angless[i].get_buffer());
for ( unsigned int i = 0; i < times.length(); i++ ) v_tms.push_back(times[i]);
return m_seq->setJointAnglesSequence(v_poss, v_tms);
}
bool SequencePlayer::setJointAnglesSequenceFull(const OpenHRP::dSequenceSequence i_jvss, const OpenHRP::dSequenceSequence i_vels, const OpenHRP::dSequenceSequence i_torques, const OpenHRP::dSequenceSequence i_poss, const OpenHRP::dSequenceSequence i_rpys, const OpenHRP::dSequenceSequence i_accs, const OpenHRP::dSequenceSequence i_zmps, const OpenHRP::dSequenceSequence i_wrenches, const OpenHRP::dSequenceSequence i_optionals, const dSequence i_tms)
{
if ( m_debugLevel > 0 ) {
std::cerr << __PRETTY_FUNCTION__ << std::endl;
}
Guard guard(m_mutex);
if (!setInitialState()) return false;
int len = i_jvss.length();
std::vector<const double*> v_jvss, v_vels, v_torques, v_poss, v_rpys, v_accs, v_zmps, v_wrenches, v_optionals;
std::vector<double> v_tms;
for ( unsigned int i = 0; i < i_jvss.length(); i++ ) v_jvss.push_back(i_jvss[i].get_buffer());
for ( unsigned int i = 0; i < i_vels.length(); i++ ) v_vels.push_back(i_vels[i].get_buffer());
for ( unsigned int i = 0; i < i_torques.length(); i++ ) v_torques.push_back(i_torques[i].get_buffer());
for ( unsigned int i = 0; i < i_poss.length(); i++ ) v_poss.push_back(i_poss[i].get_buffer());
for ( unsigned int i = 0; i < i_rpys.length(); i++ ) v_rpys.push_back(i_rpys[i].get_buffer());
for ( unsigned int i = 0; i < i_accs.length(); i++ ) v_accs.push_back(i_accs[i].get_buffer());
for ( unsigned int i = 0; i < i_zmps.length(); i++ ) v_zmps.push_back(i_zmps[i].get_buffer());
for ( unsigned int i = 0; i < i_wrenches.length(); i++ ) v_wrenches.push_back(i_wrenches[i].get_buffer());
for ( unsigned int i = 0; i < i_optionals.length(); i++ ) v_optionals.push_back(i_optionals[i].get_buffer());
for ( unsigned int i = 0; i < i_tms.length(); i++ ) v_tms.push_back(i_tms[i]);
return m_seq->setJointAnglesSequenceFull(v_jvss, v_vels, v_torques, v_poss, v_rpys, v_accs, v_zmps, v_wrenches, v_optionals, v_tms);
}
void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory(
trajectory_msgs::JointTrajectory trajectory)
{
parent->m_mutex.lock();
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @onJointTrajectoryAction (" << this->groupname << ")");
// TODO: check size and joint names
OpenHRP::dSequenceSequence angles;
OpenHRP::dSequence duration;
angles.length(trajectory.points.size());
duration.length(trajectory.points.size());
std::vector<std::string> joint_names = trajectory.joint_names;
if (joint_names.size() < joint_list.size())
{
ROS_ERROR_STREAM(
"[" << parent->getInstanceName() << "] @onJointTrajectoryAction / Error : " << "required joint_names.size() = " << joint_names.size() << " < joint_list.size() = " << joint_list.size());
return;
}
for (unsigned int i = 0; i < joint_list.size(); i++)
{
if (count(joint_names.begin(), joint_names.end(), joint_list[i]) != 1)
{
ROS_ERROR_STREAM(
"[" << parent->getInstanceName() << "] @onJointTrajectoryAction / Error : " << "joint : " << joint_list[i] << " did not exist in the required trajectory.");
return;
}
}
ROS_INFO_STREAM(
"[" << parent->getInstanceName() << "] @onJointTrajectoryAction (" << this->groupname << ") : trajectory.points.size() " << trajectory.points.size());
for (unsigned int i = 0; i < trajectory.points.size(); i++)
{
angles[i].length(joint_names.size());
trajectory_msgs::JointTrajectoryPoint point = trajectory.points[i];
for (unsigned int j = 0; j < joint_names.size(); j++)
{
parent->body->link(joint_names[j])->q = point.positions[j];
}
parent->body->calcForwardKinematics();
std::stringstream ss;
for (unsigned int j = 0; j < joint_list.size(); j++)
{
angles[i][j] = parent->body->link(joint_list[j])->q;
ss << " " << point.positions[j];
}
ROS_INFO_STREAM(
"[" << parent->getInstanceName() << "] @onJointTrajectoryAction (" << this->groupname << ") : time_from_start " << trajectory.points[i].time_from_start.toSec());
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] " << ss.str());
if (i > 0)
{
duration[i] = trajectory.points[i].time_from_start.toSec() - trajectory.points[i - 1].time_from_start.toSec();
}
else
{ // if i == 0
if (trajectory.points.size() == 1)
{
duration[i] = trajectory.points[i].time_from_start.toSec();
}
else
{ // 0.2 is magic number writtein in roseus/euslisp/robot-interface.l
duration[i] = trajectory.points[i].time_from_start.toSec() - 0.2;
}
}
}
parent->m_mutex.unlock();
if (duration.length() == 1)
{
if (groupname.length() > 0)
{ // group
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] setJointAnglesOfGroup: " << groupname);
parent->m_service0->setJointAnglesOfGroup(groupname.c_str(), angles[0], duration[0]);
}
else
{ // fullbody
parent->m_service0->setJointAngles(angles[0], duration[0]);
}
}
else
{
if (groupname.length() > 0)
{ // group
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] playPatternGroup: " << groupname);
parent->m_service0->playPatternOfGroup(groupname.c_str(), angles, duration);
}
else
{
OpenHRP::dSequenceSequence rpy, zmp;
parent->m_service0->playPattern(angles, rpy, zmp, duration);
}
}
interpolationp = true;
}