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