PoseQualityMeasurement::PoseQualityMeasurement(VirtualRobot::RobotNodeSetPtr rns)
:rns(rns)
{
THROW_VR_EXCEPTION_IF( (!rns || !rns->getTCP()), "NULL data");
name = "PoseQualityMeasurement";
considerObstacle = false;
verbose = false;
}
bool SimoxRobotViewer::showPath( const std::string &pathName, VirtualRobot::TrajectoryPtr path, VirtualRobot::VisualizationFactory::Color c )
{
removePath(pathName);
if (!path)
return false;
lock();
VirtualRobot::RobotNodeSetPtr rns = path->getRobotNodeSet();
if (!rns)
{
VR_ERROR << "No rns" << endl;
unlock();
return false;
}
RobotNodePtr tcp = rns->getTCP();
if (!tcp)
{
VR_ERROR << "No tcp" << endl;
unlock();
return false;
}
SoSeparator *sep = new SoSeparator;
sep->addChild(VirtualRobot::CoinVisualizationFactory::getCoinVisualization(path));
//Saba::CoinRrtWorkspaceVisualization cv(robot,cspace,tcp->getName());
//cv.setCustomColor(c.r,c.g,c.b);
//cv.addCSpacePath(path,Saba::CoinRrtWorkspaceVisualization::eCustom);
//SoSeparator *sep = cv.getCoinVisualization();
if (!sep)
{
unlock();
return false;
}
pathData pd;
pd.trajectory = path;
pd.visu = sep;
paths[pathName] = pd;
// add visu
sceneSep->addChild(sep);
unlock();
return true;
}
bool SimoxRobotViewer::showPath( const std::string &pathName, std::vector< std::vector<float> > path, const std::string &rns, VirtualRobot::VisualizationFactory::Color c )
{
removePath(pathName);
lock();
if (path.size()==0 || !robot || !robot->hasRobotNodeSet(rns))
{
unlock();
return false;
}
VirtualRobot::RobotNodeSetPtr rnsP = robot->getRobotNodeSet(rns);
int joints = rnsP->getSize();
if (joints<=0)
{
unlock();
return false;
}
VirtualRobot::TrajectoryPtr tr(new VirtualRobot::Trajectory(rnsP,pathName));
for (size_t i=0;i<path.size();i++)
{
if (path[i].size() != joints)
{
cout << "internal error. Config " << i << " has wrong size:" << path[i].size() << ". Expecting " << joints << endl;
continue;
}
Eigen::VectorXf c(joints);
for (int j=0;j<joints;j++)
c[j] = path[i][j];
//cspacePath->addPathPoint(c);
tr->addPoint(c);
}
unlock();
return showPath(pathName,tr,c);
}
void transformOrientationToGroundFrame(const VirtualRobot::RobotPtr& robot,
const Eigen::Matrix6Xf& leftFootTrajectory,
const VirtualRobot::RobotNodePtr& leftFoot,
const VirtualRobot::RobotNodePtr& rightFoot,
const VirtualRobot::RobotNodeSetPtr& bodyJoints,
const Eigen::MatrixXf& bodyTrajectory,
const Eigen::Matrix3Xf& trajectory,
const std::vector<SupportInterval>& intervals,
Eigen::Matrix3Xf& relativeTrajectory)
{
Eigen::Matrix4f leftInitialPose = bodyJoints->getKinematicRoot()->getGlobalPose();
int N = trajectory.cols();
int M = trajectory.rows();
relativeTrajectory.resize(M, N);
BOOST_ASSERT(M > 0 && M <= 3);
auto intervalIter = intervals.begin();
for (int i = 0; i < N; i++)
{
while (i >= intervalIter->endIdx)
{
intervalIter = std::next(intervalIter);
}
// Move basis along with the left foot
Eigen::Matrix4f leftFootPose = Eigen::Matrix4f::Identity();
VirtualRobot::MathTools::posrpy2eigen4f(1000 * leftFootTrajectory.block(0, i, 3, 1),
leftFootTrajectory.block(3, i, 3, 1), leftFootPose);
robot->setGlobalPose(leftFootPose);
bodyJoints->setJointValues(bodyTrajectory.col(i));
Eigen::Matrix3f worldToRef = computeGroundFrame(leftFoot->getGlobalPose(),
rightFoot->getGlobalPose(),
intervalIter->phase).block(0, 0, 3, 3);
relativeTrajectory.block(0, i, M, 1) = worldToRef.colPivHouseholderQr().solve(trajectory.col(i)).block(0, 0, M, 1);
}
}
void extractControlFrames(VirtualRobot::RobotPtr robot,
const Eigen::Matrix6Xf& leftFootTrajectory,
const Eigen::MatrixXf& bodyTrajectory,
VirtualRobot::RobotNodeSetPtr bodyJoints,
VirtualRobot::RobotNodePtr node,
std::vector<Eigen::Matrix4f>& controlFrames)
{
int N = leftFootTrajectory.cols();
for (int i = 0; i < N; i++)
{
// Move basis along with the left foot
Eigen::Matrix4f leftFootPose = Eigen::Matrix4f::Identity();
VirtualRobot::MathTools::posrpy2eigen4f(1000 * leftFootTrajectory.block(0, i, 3, 1),
leftFootTrajectory.block(3, i, 3, 1), leftFootPose);
robot->setGlobalPose(leftFootPose);
bodyJoints->setJointValues(bodyTrajectory.col(i));
controlFrames.push_back(node->getGlobalPose());
}
}
float SupportPolygon::getStabilityIndex(VirtualRobot::RobotNodeSetPtr rns, bool update)
{
if (!rns)
{
return 0.0f;
}
if (update)
{
updateSupportPolygon();
}
// check if com is outside support polygon
MathTools::ConvexHull2DPtr ch = getSupportPolygon2D();
if (!ch || ch->vertices.size() < 2)
{
return 0.0f;
}
Eigen::Vector2f com2D = rns->getCoM().head(2);
if (!MathTools::isInside(com2D, ch))
{
//cout << "CoM outside of support polygon" << endl;
return 0.0f;
}
// compute min distance center<->border
// and min distance com to line segment
Eigen::Vector2f center = MathTools::getConvexHullCenter(ch);
float minDistCenter = FLT_MAX;
float minDistCoM = FLT_MAX;
Eigen::Vector2f pt1, pt2;
for (size_t i = 0; i < ch->vertices.size(); i++)
{
if (i > 0)
{
pt1 = ch->vertices[i - 1];
pt2 = ch->vertices[i];
}
else
{
pt1 = ch->vertices[ch->vertices.size() - 1];
pt2 = ch->vertices[0];
}
float d = getSquaredDistLine(center, pt1, pt2);
//(ch->vertices[i]-center).squaredNorm();
if (d < minDistCenter)
{
minDistCenter = d;
}
//distPointLine
d = getSquaredDistLine(com2D, pt1, pt2);
//(ptOnLine-com2D).squaredNorm();
if (d < minDistCoM)
{
/*cout << "minDistCom:" << sqrtf(d) << ", com2D:" << com2D(0) << "," << com2D(1) << endl;
cout << "minDistCom:" << sqrtf(d) << ", pt1:" << pt1(0) << "," << pt1(1) << endl;
cout << "minDistCom:" << sqrtf(d) << ", pt2:" << pt2(0) << "," << pt2(1) << endl;
cout << "minDistCom:" << sqrtf(d) << ", ptOnLine:" << ptOnLine(0) << "," << ptOnLine(1) << endl;*/
minDistCoM = d;
}
}
minDistCenter = sqrtf(minDistCenter);
minDistCoM = sqrtf(minDistCoM);
//cout << "Dist center -> border:" << minDistCenter << endl;
//cout << "Dist com -> border:" << minDistCoM << endl;
float res = 0.0f;
if (fabs(minDistCenter) > 1e-8)
{
res = minDistCoM / minDistCenter;
}
//cout << "Stability Value:" << res << endl;
if (res > 1.0f)
{
res = 1.0f;
}
return res;
}