Esempio n. 1
0
std::vector<Eigen::Matrix4f > Trajectory::createWorkspaceTrajectory( VirtualRobot::RobotNodePtr r )
{
	VR_ASSERT(rns);
	if (!r)
		r = rns->getTCP();
	VR_ASSERT(r);

	RobotPtr robot = rns->getRobot();
	VR_ASSERT(robot);

	Eigen::VectorXf jv;
	rns->getJointValues(jv);

	std::vector<Eigen::Matrix4f > result;

	for (size_t i = 0; i < path.size(); i++)
	{
		// get tcp coords:
		robot->setJointValues(rns,path[i]);
		Eigen::Matrix4f m;
		result.push_back(r->getGlobalPose());
	}
	robot->setJointValues(rns,jv);
	return result;
}
Esempio n. 2
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);
    }
}
Esempio n. 3
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());
    }
}
void GraspPlannerEvaluatorWindow::perturbatedGrasp()
{
	openEEF();
	resetPose();

	graspNumber = UI.spinBoxGraspNum->value();
	if (graspNumber <= 0 || grasps->getSize() < graspNumber) {
		graspNumber = grasps->getSize();
		UI.spinBoxGraspNum->setValue(graspNumber);
		std::cout << "Invalid grasp number selection! Settting to last planned grasp." << std::endl;
	}

	// set to last valid grasp
	if (graspNumber > 0 && eefCloned && eefCloned->getEndEffector(eefName))
	{
		Eigen::Matrix4f mGrasp = grasps->getGrasp(graspNumber - 1)->getTcpPoseGlobal(object->getGlobalPose());
		eefCloned->setGlobalPoseForRobotNode(eefCloned->getEndEffector(eefName)->getTcp(), mGrasp);
		if (!UI.doubleSpinBoxStep->value() && !UI.doubleSpinBoxVDelay->value() ){
                  perturbateObject();
                  
                  //Grasp Center Point Robot Node
                  VirtualRobot::RobotNodePtr graspNode = eefCloned->getEndEffector(eefName)->getGCP();
                  //Grasp Pose
                  Eigen::Matrix4f pose = graspNode->getGlobalPose();
                  //Approach Direction (-z vector)
                  Eigen::Vector3f approachDir = -pose.block(0,0,3,1);
                  
                  //Move eff away	
                  moveEEFAway(approachDir, 3.0f);
                  closeEEF();
                }
                else
                {
                  	Eigen::Vector3f rotPertub;
                        rotPertub.setRandom(3).normalize();
                        Eigen::Vector3f translPertub;
                        translPertub.setRandom(3).normalize();
                        Eigen::Matrix4f deltaPose;
                        deltaPose.setIdentity();
                        std::cout<<object->getGlobalPose()<<std::endl;  
                        
                        float step=UI.doubleSpinBoxStep->value();
                        float delay =UI.doubleSpinBoxVDelay->value(); //Delay in secs
                        //float min_error=0;
                        
                        //check id step is valid
                        //
                        Eigen::Matrix4f temp;
                        temp=object->getGlobalPose(); 
                        VirtualRobot::RobotNodePtr graspNode = eefCloned->getEndEffector(eefName)->getGCP();
                        for (step;step<UI.doubleSpinBoxPertDistanceX->value();step++){
                          translPertub(0) =  step;
                          translPertub(1) = 0;
                          translPertub(2) = 0;
                          //std::cout<<step<<std::endl;
                          //std::cout<<UI.doubleSpinBoxPertDistanceX->value()<<std::endl;
                          deltaPose.block(0,3,3,1) = translPertub;
                          
/*                           std::cout<< "DeltaPose:\n" << deltaPose << std::endl; */
                          // std::cout << "Pose:\n" << object->getGlobalPose() << std::endl;
                          /* std::cout << "FinalPose:\n" << (deltaPose*temp) << std::endl; */
                          
                          object->setGlobalPose(deltaPose*temp);
                          //  VirtualRobot::RobotNodePtr graspNode = eefCloned->getEndEffector(eefName)->getGCP();
                          
                          //Grasp Pose
                          //Eigen::Matrix4f pose = graspNode->getGlobalPose();
                          //Approach Direction (-z vector)
                          //Eigen::Vector3f approachDir = -pose.block(0,0,3,1);
                          //Move eff away	
                          //moveEEFAway(approachDir, 3.0f); 
                         
                          closeEEF(); 
                          contacts.clear();
                          contacts = eefCloned->getEndEffector(eefName)->closeActors(object);
                          qualityMeasure->setContactPoints(contacts);
                          float qual = qualityMeasure->getGraspQuality();
                          bool isFC = qualityMeasure->isGraspForceClosure();
                          //if (qual<=UI.doubleSpinBoxError->value()) break;
                          std::cout << "Grasp Nr " << graspNumber << "\nQuality (wrench space): " << qual << "\nForce closure: ";
		          if (isFC){
                            std::cout << "yes"<<std::endl;
                          }
                          else{
                            std::cout << "no"<<std::endl;
                          }
                          viewer->render();
                          sleep(delay/1000);
                        }
                }
        }
}