Пример #1
0
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();
}
Пример #2
0
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
}
Пример #3
0
    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;
    }