PoseQualityMeasurement::PoseQualityMeasurement(VirtualRobot::RobotNodeSetPtr rns)
:rns(rns)
{
	THROW_VR_EXCEPTION_IF( (!rns || !rns->getTCP()), "NULL data");
	name = "PoseQualityMeasurement";
	considerObstacle = false;
	verbose = false;
}
Example #2
0
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;
}
Example #3
0
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);
}
Example #4
0
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);
    }
}
Example #5
0
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());
    }
}
Example #6
0
    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;
    }