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(); }
void showRobotWindow::jointValueChanged(int pos) { int nr = UI.comboBoxJoint->currentIndex(); if (nr<0 || nr>=(int)currentRobotNodes.size()) return; float fPos = currentRobotNodes[nr]->getJointLimitLo() + (float)pos / 1000.0f * (currentRobotNodes[nr]->getJointLimitHi() - currentRobotNodes[nr]->getJointLimitLo()); robot->setJointValue(currentRobotNodes[nr],fPos); UI.lcdNumberJointValue->display((double)fPos); #if 0 RobotNodePtr rnl = robot->getRobotNode("LeftLeg_TCP"); RobotNodePtr rnr = robot->getRobotNode("RightLeg_TCP"); if (rnl && rnr) { cout << "LEFT:" << endl; MathTools::printMat(rnl->getGlobalPose()); cout << "RIGHT:" << endl; MathTools::printMat(rnr->getGlobalPose()); } #endif }
Eigen::MatrixXf CoMIK::getJacobianOfCoM(RobotNodePtr node) { // Get number of degrees of freedom size_t nDoF = rns->getAllRobotNodes().size(); // Create matrices for the position and the orientation part of the jacobian. Eigen::MatrixXf position = Eigen::MatrixXf::Zero(3, nDoF); const std::vector<RobotNodePtr> parentsN = bodyNodeParents[node]; // Iterate over all degrees of freedom for (size_t i = 0; i < nDoF; i++) { RobotNodePtr dof = rns->getNode(i);// bodyNodes[i]; // Check if the tcp is affected by this DOF if (find(parentsN.begin(), parentsN.end(), dof) != parentsN.end()) { // Calculus for rotational joints is different as for prismatic joints. if (dof->isRotationalJoint()) { // get axis boost::shared_ptr<RobotNodeRevolute> revolute = boost::dynamic_pointer_cast<RobotNodeRevolute>(dof); THROW_VR_EXCEPTION_IF(!revolute, "Internal error: expecting revolute joint"); // todo: find a better way of handling different joint types Eigen::Vector3f axis = revolute->getJointRotationAxis(coordSystem); // For CoM-Jacobians only the positional part is necessary Eigen::Vector3f toTCP = node->getCoMLocal() + node->getGlobalPose().block(0, 3, 3, 1) - dof->getGlobalPose().block(0, 3, 3, 1); position.block(0, i, 3, 1) = axis.cross(toTCP); } else if (dof->isTranslationalJoint()) { // -> prismatic joint boost::shared_ptr<RobotNodePrismatic> prismatic = boost::dynamic_pointer_cast<RobotNodePrismatic>(dof); THROW_VR_EXCEPTION_IF(!prismatic, "Internal error: expecting prismatic joint"); // todo: find a better way of handling different joint types Eigen::Vector3f axis = prismatic->getJointTranslationDirection(coordSystem); position.block(0, i, 3, 1) = axis; } } } if (target.rows() == 2) { Eigen::MatrixXf result(2, nDoF); result.row(0) = position.row(0); result.row(1) = position.row(1); return result; } else if (target.rows() == 1) { VR_INFO << "One dimensional CoMs not supported." << endl; } return position; }