Eigen::Matrix4f DynamicsRobot::getComGlobal( VirtualRobot::RobotNodePtr rn ) { Eigen::Matrix4f com = Eigen::Matrix4f::Identity(); com.block(0,3,3,1) = rn->getCoMLocal(); com = rn->getGlobalPoseVisualization()*com; return com; }
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; }
void thread2(){ for (int i=0; i < 20;i++){ r1->print(); VirtualRobot::LinkedCoordinate coord(rob); coord.set(r1); std::cout << coord.getInFrame(r2).block<3,1>(0,3) << std::endl; } }
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()); } }
bool SimoxRobotViewer::convertCoords( const std::string &from, const std::string &to, Eigen::Matrix4f &m ) { if (!robot) return false; bool fromGlobal = from.empty(); bool toGlobal = to.empty(); if (fromGlobal && toGlobal) return true; lock(); VirtualRobot::RobotNodePtr f; if (!fromGlobal) { if (!robot->hasRobotNode(from)) { VR_ERROR << "no robot node with name " << from << endl; unlock(); return false; } f = robot->getRobotNode(from); } VirtualRobot::RobotNodePtr t; if (!toGlobal) { if (!robot->hasRobotNode(to)) { VR_ERROR << "no robot node with name " << to << endl; unlock(); return false; } t = robot->getRobotNode(to); } if (fromGlobal) { if (!t) { unlock(); return false; } m = t->toLocalCoordinateSystem(m); unlock(); return true; } if (toGlobal) { if (!f) { unlock(); return false; } m = f->toGlobalCoordinateSystem(m); unlock(); return true; } // convert from f to t if (!f || !t) { unlock(); return false; } m = f->toGlobalCoordinateSystem(m); m = t->toLocalCoordinateSystem(m); unlock(); return true; }
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); } } } }