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