void SimoxMotionState::setGlobalPoseSimox( const Eigen::Matrix4f& worldPose )
{
if (!sceneObject)
return;
// inv com as matrix4f
Eigen::Matrix4f comLocal;
comLocal.setIdentity();
comLocal.block(0,3,3,1) = -com;
// worldPose -> local visualization frame
/*Eigen::Matrix4f localPose = sceneObject->getGlobalPoseVisualization().inverse() * worldPose;
// com as matrix4f
Eigen::Matrix4f comLocal;
comLocal.setIdentity();
comLocal.block(0,3,3,1) = -com;
//Eigen::Matrix4f localPoseAdjusted = localPose * comLocal;
//Eigen::Matrix4f localPoseAdjusted = comLocal * localPose;
//Eigen::Matrix4f localPoseAdjusted = localPose;
//localPoseAdjusted.block(0,3,3,1) -= com;
Eigen::Matrix4f comTrafo = Eigen::Matrix4f::Identity();
comTrafo.block(0,3,3,1) = -com;
Eigen::Matrix4f localPoseAdjusted = localPose * comTrafo;
//localPoseAdjusted.block(0,3,3,1) -= com;
Eigen::Matrix4f resPose = sceneObject->getGlobalPoseVisualization() * localPoseAdjusted;
*/
Eigen::Matrix4f resPose = worldPose * comLocal;
if (robotNodeActuator)
{
// get joint angle
RobotNodePtr rn = robotNodeActuator->getRobotNode();
DynamicsWorldPtr w = DynamicsWorld::GetWorld();
DynamicsRobotPtr dr = w->getEngine()->getRobot(rn->getRobot());
BulletRobotPtr bdr = boost::dynamic_pointer_cast<BulletRobot>(dr);
if (bdr)
{
// check if robotnode is connected with a joint
std::vector<BulletRobot::LinkInfo> links = bdr->getLinks(rn);
// update all involved joint values
for (size_t i=0;i<links.size();i++)
{
if (links[i].nodeJoint)
{
float ja = bdr->getJointAngle(links[i].nodeJoint);
// we can update the joint value via an RobotNodeActuator
RobotNodeActuatorPtr rna (new RobotNodeActuator(links[i].nodeJoint));
rna->updateJointAngle(ja);
}
}
// we assume that all models are handled by Bullet, so we do not need to update children
robotNodeActuator->updateVisualizationPose(resPose, false);
#if 0
if (rn->getName() == "Shoulder 1 L")
{
cout << "Shoulder 1 L:" << ja << ", speed:" << bdr->getJointSpeed(rn) << endl;
}
#endif
} /*else
{
VR_WARNING << "Could not determine dynamic robot?!" << endl;
}*/
} else
{
sceneObject->setGlobalPose(resPose);
}
}
Eigen::Matrix4f LinkedCoordinate::getCoordinateTransformation(const RobotNodePtr &origin,
const RobotNodePtr &destination, const RobotPtr &robot){
if (!destination)
THROW_VR_EXCEPTION(format("Destination RobotNodePtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
if (!origin)
THROW_VR_EXCEPTION(format("Origin RobotNodePtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
if (!robot)
THROW_VR_EXCEPTION(format("RobotPtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
if (!robot->hasRobotNode( origin) )
THROW_VR_EXCEPTION(format("Origin robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % origin->getName() % robot->getName() % BOOST_CURRENT_FUNCTION);
if (!robot->hasRobotNode( destination) )
THROW_VR_EXCEPTION(format("Destination robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % destination->getName() % robot->getName() % BOOST_CURRENT_FUNCTION);
// std::cout << "Destination: " << destination->getName() <<std::endl << "Origin: " << origin->getName() << std::endl;
// std::cout << "Destination:\n" << destination->getGlobalPose() <<std::endl << "Origin:\n" << origin->getGlobalPose() << std::endl;
return destination->getGlobalPose().inverse() * origin->getGlobalPose();
}
Eigen::Matrix4f LinkedCoordinate::getInFrame(const RobotNodePtr & destination) const {
if (!destination)
THROW_VR_EXCEPTION(format("RobotNodePtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
if (!this->robot->hasRobotNode( destination) )
THROW_VR_EXCEPTION(format("Robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % destination->getName() % this->robot->getName() % BOOST_CURRENT_FUNCTION);
return LinkedCoordinate::getCoordinateTransformation(this->frame,destination,this->robot) * this->pose;
}
void LinkedCoordinate::changeFrame(const RobotNodePtr & destination) {
if (!destination)
THROW_VR_EXCEPTION(format("RobotNodePtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
if (!this->robot->hasRobotNode( destination) )
THROW_VR_EXCEPTION(format("Robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % destination->getName() % this->robot->getName() % BOOST_CURRENT_FUNCTION);
if (!this->frame)
this->pose = Eigen::Matrix4f::Identity();
else
this->pose = LinkedCoordinate::getCoordinateTransformation(this->frame,destination,this->robot) * this->pose;
this->frame = destination;
}
void LinkedCoordinate::set( const RobotNodePtr &frame,const Eigen::Matrix4f &pose) {
if (!frame)
THROW_VR_EXCEPTION(format("RobotNodePtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
if (!this->robot->hasRobotNode( frame ) )
THROW_VR_EXCEPTION(format("Robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % frame->getName() % this->robot->getName() % BOOST_CURRENT_FUNCTION);
this->pose = pose;
this->frame = frame;
}
void reachabilityWindow::createReach()
{
if (!robot || !currentRobotNodeSet)
return;
// setup window
Ui::ReachabilityCreate UICreate;
QDialog diag;
UICreate.setupUi(&diag);
RobotNodePtr baseNode = currentRobotNodeSet->getKinematicRoot();
RobotNodePtr tcpNode = currentRobotNodeSet->getTCP();
UICreate.labelRNS->setText(QString("RobotNodeSet: ") + QString(currentRobotNodeSet->getName().c_str()));
UICreate.labelBaseNode->setText(QString("Base: ") + QString(baseNode->getName().c_str()));
UICreate.labelTCP->setText(QString("TCP: ") + QString(tcpNode->getName().c_str()));
reachSpace.reset(new Reachability(robot));
float minB[6];// = {-1000.0f,-1000.0f,-1000.0f,(float)-M_PI,(float)-M_PI,(float)-M_PI};
float maxB[6];// ={1000.0f,1000.0f,1000.0f,(float)M_PI,(float)M_PI,(float)M_PI};
reachSpace->checkForParameters(currentRobotNodeSet,1000,minB,maxB,baseNode,tcpNode);
//float ex = currentRobotNodeSet->getMaximumExtension();
UICreate.doubleSpinBoxMinX->setValue(minB[0]);
UICreate.doubleSpinBoxMaxX->setValue(maxB[0]);
UICreate.doubleSpinBoxMinY->setValue(minB[1]);
UICreate.doubleSpinBoxMaxY->setValue(maxB[1]);
UICreate.doubleSpinBoxMinZ->setValue(minB[2]);
UICreate.doubleSpinBoxMaxZ->setValue(maxB[2]);
std::vector < VirtualRobot::RobotNodeSetPtr > allRNS;
robot->getRobotNodeSets(allRNS);
for (size_t i=0;i<allRNS.size();i++)
{
UICreate.comboBoxColModelDynamic->addItem(QString(allRNS[i]->getName().c_str()));
UICreate.comboBoxColModelStatic->addItem(QString(allRNS[i]->getName().c_str()));
}
if (diag.exec())
{
minB[0] = UICreate.doubleSpinBoxMinX->value();
minB[1] = UICreate.doubleSpinBoxMinY->value();
minB[2] = UICreate.doubleSpinBoxMinZ->value();
minB[3] = UICreate.doubleSpinBoxMinRo->value();
minB[4] = UICreate.doubleSpinBoxMinPi->value();
minB[5] = UICreate.doubleSpinBoxMinYa->value();
maxB[0] = UICreate.doubleSpinBoxMaxX->value();
maxB[1] = UICreate.doubleSpinBoxMaxY->value();
maxB[2] = UICreate.doubleSpinBoxMaxZ->value();
maxB[3] = UICreate.doubleSpinBoxMaxRo->value();
maxB[4] = UICreate.doubleSpinBoxMaxPi->value();
maxB[5] = UICreate.doubleSpinBoxMaxYa->value();
SceneObjectSetPtr staticModel;
SceneObjectSetPtr dynamicModel;
if (UICreate.checkBoxColDetecion->isChecked())
{
std::string staticM = std::string(UICreate.comboBoxColModelStatic->currentText().toAscii());
std::string dynM = std::string(UICreate.comboBoxColModelDynamic->currentText().toAscii());
staticModel = robot->getRobotNodeSet(staticM);
dynamicModel = robot->getRobotNodeSet(dynM);
}
float discrTr = UICreate.doubleSpinBoxDiscrTrans->value();
float discrRo = UICreate.doubleSpinBoxDiscrRot->value();
reachSpace->initialize(currentRobotNodeSet,discrTr,discrRo,minB,maxB,staticModel,dynamicModel,baseNode,tcpNode);//200.0f,0.4f,minB,maxB,staticModel,dynamicModel,baseNode);
reachSpace->print();
reachSpace->addCurrentTCPPose();
reachSpace->print();
}
}
