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