Exemple #1
0
void ZMPConstraint::Plot(const DblVec& x, OR::EnvironmentBase& env, std::vector<OR::GraphHandlePtr>& handles) {
  Matrix<float, Dynamic, 3, RowMajor> xyz(m_ab.rows()*2, 3);
  xyz.col(2).setZero();
  int npts = m_pts.rows();
  for (int i=0; i < npts; ++i) {
    xyz(2*i,0) = m_pts(i,0);
    xyz(2*i,1) = m_pts(i,1);
    xyz(2*i+1,0) = m_pts((i+1)%npts,0);
    xyz(2*i+1,1) = m_pts((i+1)%npts,1);
  }
  handles.push_back(env.drawlinelist(xyz.data(), xyz.rows(), sizeof(float)*3, 4, OR::RaveVector<float>(1,0,0,1)));

  m_rad->SetDOFValues(getDblVec(x,m_vars));
  OR::Vector moment(0,0,0);
  float totalmass = 0;
  BOOST_FOREACH(const KinBody::LinkPtr& link, m_rad->GetRobot()->GetLinks()) {
    if (!link->GetGeometries().empty()) {
      moment += link->GetGlobalCOM() * link->GetMass();
      totalmass += link->GetMass();
    }
  }
  moment /= totalmass;
  OR::Vector moment_ground = moment; moment_ground.z = 0;
  handles.push_back(env.drawarrow(moment, moment_ground, .005, OR::RaveVector<float>(1,0,0,1)));
}
Exemple #2
0
void FaceContactConstraint::Plot(const DblVec& xvec, OR::EnvironmentBase& env, std::vector<OR::GraphHandlePtr>& handles) {
  FaceContactErrorCalculator* calc = static_cast<FaceContactErrorCalculator*>(f_.get());
  OR::Vector ptA, ptB;
  VectorXd x = getVec(xvec, vars_);
  calc->m_config->SetDOFValues(toDblVec(x.topRows(calc->m_nDof)));
  calc->CalcWorldPoints(x, ptA, ptB);
  handles.push_back(env.drawarrow(ptA, ptB, .001, OR::Vector(1, 0, 1, 1)));
}
Exemple #3
0
void CartPoseCost::Plot(const DblVec& x, OR::EnvironmentBase& env, std::vector<OR::GraphHandlePtr>& handles) {
  CartPoseErrCalculator* calc = static_cast<CartPoseErrCalculator*>(f_.get());
  DblVec dof_vals = getDblVec(x, vars_);
  calc->manip_->SetDOFValues(dof_vals);
  OR::Transform target = calc->pose_inv_.inverse(), cur = calc->link_->GetTransform();
  PlotAxes(env, cur, .1,  handles);
  PlotAxes(env, target, .1,  handles);
  handles.push_back(env.drawarrow(cur.trans, target.trans, .01, OR::Vector(1,0,1,1)));
}
boost::shared_ptr<CollisionChecker> CollisionChecker::GetOrCreate(OR::EnvironmentBase& env) {
  UserDataPtr ud = GetUserData(env, "trajopt_cc");
  if (!ud) {
    LOG_INFO("creating bullet collision checker for environment");
     //throw std::runtime_error("not implemented");
    ud =  CreateCollisionChecker(env.shared_from_this());
    SetUserData(env, "trajopt_cc", ud);
  }
  else {
    LOG_DEBUG("already have a collision checker for this environment");
  }
  return boost::dynamic_pointer_cast<CollisionChecker>(ud);
}
  BeliefCollisionCheckerPtr BeliefCollisionChecker::GetOrCreate(OR::EnvironmentBase& env) {
    UserDataPtr ud = GetUserData(env, "trajopt_cc");
    if (!ud) {
      LOG_INFO("creating bullet belief collision checker for environment");
      BeliefCollisionCheckerPtr checker(new BeliefCollisionChecker(env.shared_from_this()));
      ud = checker;
      SetUserData(env, "trajopt_cc", ud);
    }
    else {
      LOG_DEBUG("already have a belief collision checker for this environment");
    }
    return boost::dynamic_pointer_cast<BeliefCollisionChecker>(ud);

  }