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;
}
Example #2
0
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);
}
Example #3
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);
}
Example #4
0
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;
}