Example #1
0
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;
}
Example #2
0
    VirtualRobot::CollisionModelPtr CollisionModel::CreateUnitedCollisionModel(const std::vector<CollisionModelPtr>& colModels)
    {
        VR_ASSERT(colModels.size() > 0);
        CollisionCheckerPtr colChecker = colModels[0]->getCollisionChecker();
        std::vector<VisualizationNodePtr> visus;

        for (size_t i = 0; i < colModels.size(); i++)
        {
            VisualizationNodePtr v = colModels[i]->getVisualization();

            if (v)
            {
                visus.push_back(v);
            }

            VR_ASSERT(colModels[i]->getCollisionChecker() == colChecker);
        }

        if (visus.size() == 0)
        {
            return CollisionModelPtr();
        }

        VisualizationNodePtr vc = VisualizationNode::CreateUnitedVisualization(visus);
        return CollisionModelPtr(new CollisionModel(vc, "", colChecker));
    }
Example #3
0
void DynamicsRobot::createDynamicsNode( VirtualRobot::RobotNodePtr node )
{
	VR_ASSERT(node);

	// check if already created
	if (hasDynamicsRobotNode(node))
		return;

	DynamicsWorldPtr dw = DynamicsWorld::GetWorld();
	DynamicsObjectPtr drn = dw->CreateDynamicsObject(node);
	VR_ASSERT(drn);
	dynamicRobotNodes[node] = drn;
}
Example #4
0
void DynamicsRobot::actuateNode( VirtualRobot::RobotNodePtr node, float jointValue , float jointVelocity)
{
    VR_ASSERT(robot);
    VR_ASSERT(node);
    VR_ASSERT(robot->hasRobotNode(node));

    robotNodeActuationTarget target;
    target.actuation = ePositionVelocity;
    target.node = node;
    target.jointValueTarget = jointValue;
    target.jointVelocityTarget = jointVelocity;
    target.jointTorqueTarget = 0.0f;

    actuationTargets[node] = target;
}
Example #5
0
DynamicsObjectPtr DynamicsRobot::getDynamicsRobotNode( VirtualRobot::RobotNodePtr node )
{
	VR_ASSERT(node);
	if (dynamicRobotNodes.find(node) == dynamicRobotNodes.end())
		return DynamicsObjectPtr();
	return dynamicRobotNodes[node];
}
Example #6
0
bool DynamicsRobot::isNodeActuated( VirtualRobot::RobotNodePtr node )
{
	VR_ASSERT(node);
	if (actuationTargets.find(node) == actuationTargets.end())
		return false;
    return actuationTargets[node].actuation!=eDisabled;
}
Example #7
0
float DynamicsRobot::getNodeTarget( VirtualRobot::RobotNodePtr node )
{
    VR_ASSERT(node);
    if (actuationTargets.find(node) == actuationTargets.end())
        return 0.0f;
    return actuationTargets[node].jointValueTarget;

}
Example #8
0
void RobotNode::setJointValue(float q)
{
	RobotPtr r = getRobot();
	VR_ASSERT(r);
	WriteLockPtr lock = r->getWriteLock();
	setJointValueNoUpdate(q);
	updatePose();
}
Example #9
0
void RobotNodeSet::setJointValues( const RobotConfigPtr jointValues )
{
	VR_ASSERT(jointValues);
	RobotPtr rob = robot.lock();
	VR_ASSERT(rob);
	WriteLockPtr lock = rob->getWriteLock();
	
	for (unsigned int i=0;i<robotNodes.size();i++)
	{
		if (jointValues->hasConfig(robotNodes[i]->getName()))
			robotNodes[i]->setJointValueNoUpdate(jointValues->getConfig(robotNodes[i]->getName()));
	}
	if (kinematicRoot)
		kinematicRoot->updatePose();
	else
		rob->applyJointValues();

}
Example #10
0
void DynamicsRobot::actuateNodeTorque( VirtualRobot::RobotNodePtr node, float jointTorque )
{
    VR_ASSERT(robot);
    VR_ASSERT(node);
    VR_ASSERT(robot->hasRobotNode(node));

    //if (!hasDynamicsRobotNode(node))
    //    createDynamicsNode(node);

    //DynamicsObjectPtr dnyRN = getDynamicsRobotNode(node);

    robotNodeActuationTarget target;
    target.actuation = eTorque;
    target.node = node;
    target.jointValueTarget = 0.0f;
    target.jointVelocityTarget = 0.0f;
    target.jointTorqueTarget = jointTorque;
    //target.dynNode = dnyRN;

    actuationTargets[node] = target;
}
Example #11
0
    CoMIK::CoMIK(RobotNodeSetPtr rnsJoints, RobotNodeSetPtr rnsBodies, RobotNodePtr coordSystem, int dimensions)
	: JacobiProvider(rnsJoints), coordSystem(coordSystem)
{
	VR_ASSERT(rns);
	VR_ASSERT(rnsBodies);
	checkImprovement = false;
	this->rnsBodies = rnsBodies;
    numDimensions = dimensions;

	bodyNodes = rnsBodies->getAllRobotNodes();
	for (size_t i = 0; i < bodyNodes.size(); i++)
	{
		// get all joints that influence the body
		std::vector<RobotNodePtr> parentsN = bodyNodes[i]->getAllParents(rns);
		bodyNodeParents[bodyNodes[i]] = parentsN;
	}
	if (rnsBodies->getMass()==0)
	{
		VR_ERROR << "The RNS does not contain any bodies or masses are not specified (mass==0)" << endl;
	}
}
Example #12
0
RobotNodeSetPtr RobotNodeSet::createRobotNodeSet(RobotPtr robot, 
												 const std::string &name, 
												 const std::vector< std::string > &robotNodeNames, 
												 const std::string &kinematicRootName, 
												 const std::string &tcpName,
												 bool registerToRobot)
{
	VR_ASSERT (robot!=NULL);
	std::vector< RobotNodePtr > robotNodes;
	if (robotNodeNames.empty())
	{
		VR_WARNING << " No robot nodes in set..." << endl;
	}
	else
	{
		for (unsigned int i=0;i<robotNodeNames.size();i++)
		{
			RobotNodePtr node = robot->getRobotNode(robotNodeNames[i]);
			THROW_VR_EXCEPTION_IF(!node, "No robot node with name " << robotNodeNames[i] << " found...");
			robotNodes.push_back(node);
		}
	}

	RobotNodePtr kinematicRoot;
	if (!kinematicRootName.empty())
	{
		RobotNodePtr node = robot->getRobotNode(kinematicRootName);
		THROW_VR_EXCEPTION_IF(!node, "No root node with name " << kinematicRootName << " found...");
		kinematicRoot = node;
	}
	else
	{
		if (!robotNodes.empty())
			kinematicRoot = robotNodes[0];
	}

	RobotNodePtr tcp;
	if (!tcpName.empty())
	{
		RobotNodePtr node = robot->getRobotNode(tcpName);
		THROW_VR_EXCEPTION_IF(!node, "No tcp node with name " << tcpName << " found...");
		tcp = node;
	}
	else
	{
		if (!robotNodes.empty())
			tcp = robotNodes[robotNodes.size()-1];
	}

	RobotNodeSetPtr rns = RobotNodeSet::createRobotNodeSet(robot, name, robotNodes, kinematicRoot, tcp, registerToRobot);
	return rns;
}
Example #13
0
RobotNodeSetPtr RobotNodeSet::createRobotNodeSet(RobotPtr robot, 
												 const std::string &name, 
												 const std::vector< RobotNodePtr > &robotNodes, 
												 const RobotNodePtr kinematicRoot,
												 const RobotNodePtr tcp,
												 bool registerToRobot)
{
	VR_ASSERT (robot!=NULL);

	if (robotNodes.empty() || !robotNodes[0])
	{
		VR_WARNING << " No robot nodes in set..." << endl;
	} else
	{
		CollisionCheckerPtr cc = robotNodes[0]->getCollisionChecker();

		for (unsigned int i=1;i<robotNodes.size();i++)
		{
			if (robotNodes[0]->getRobot() != robotNodes[i]->getRobot())
			{
				THROW_VR_EXCEPTION("Robot nodes belong to different robots");
			}
			if (cc !=  robotNodes[i]->getCollisionChecker())
			{
				THROW_VR_EXCEPTION("Robot nodes belong to different collision checkers");
			}
		}
	}

	RobotNodePtr kinematicRootNode = kinematicRoot;
	if (!kinematicRootNode)
	{
		kinematicRootNode = robot->getRootNode();
	}
	RobotNodePtr tcpNode = tcp;
	if (!tcpNode)
	{
		THROW_VR_EXCEPTION_IF(robotNodes.empty(), "can not determine the tcp node need for creating a RobotNodeSet");
		tcpNode = robotNodes[robotNodes.size()-1];
	}
	RobotNodeSetPtr rns(new RobotNodeSet(name, robot, robotNodes, kinematicRootNode, tcpNode));

	if (registerToRobot)
	{
		THROW_VR_EXCEPTION_IF(robot->hasRobotNodeSet(rns), "RobotNodeSet with name " << rns->getName() << " already present in the robot");
		robot->registerRobotNodeSet(rns);
	}

	return rns;
}
Example #14
0
void DynamicsRobot::actuateNode( VirtualRobot::RobotNodePtr node, float jointValue )
{
	VR_ASSERT(robot);
	VR_ASSERT(node);
	VR_ASSERT(robot->hasRobotNode(node));

	// if node is a joint without model, there is no dyn node!
	//DynamicsObjectPtr dnyRN;
	//if (hasDynamicsRobotNode(node))
	//	dnyRN = getDynamicsRobotNode(node);
	//	createDynamicsNode(node);


    robotNodeActuationTarget target;
    target.actuation = ePosition;
    target.node = node;
    target.jointValueTarget = jointValue;
    target.jointVelocityTarget = 0.0f;
    target.jointTorqueTarget = 0.0f;
    //target.dynNode = dnyRN;

    actuationTargets[node] = target;
}
Example #15
0
void RobotNodeSet::setJointValues(const Eigen::VectorXf &jointValues)
{
	THROW_VR_EXCEPTION_IF(jointValues.rows() != robotNodes.size(), "Wrong vector dimension (robotNodes:" << robotNodes.size() << ", jointValues: " << jointValues.size() << ")" << endl);
	RobotPtr rob = robot.lock();
	VR_ASSERT(rob);
	WriteLockPtr lock = rob->getWriteLock();
	for (unsigned int i=0;i<robotNodes.size();i++)
	{
		robotNodes[i]->setJointValueNoUpdate(jointValues[i]);
	}	
	if (kinematicRoot)
		kinematicRoot->updatePose();
	else
		rob->applyJointValues();
}
Example #16
0
    SupportPolygon::SupportPolygon(SceneObjectSetPtr contactModels)
        : contactModels(contactModels)
    {
        floor =  MathTools::getFloorPlane();

        VR_ASSERT(this->contactModels);

        for (size_t i = 0; i < contactModels->getSize(); i++)
        {
            if (contactModels->getSceneObject((int)i)->getCollisionModel())
            {
                colModels.push_back(contactModels->getSceneObject((int)i)->getCollisionModel());
            }
        }

        THROW_VR_EXCEPTION_IF(colModels.size() == 0, "RobotNodeSet does not contain any collision models");
    }
Example #17
0
    VirtualRobot::TriMeshModelPtr SphereApproximator::generateTriMesh(const SphereApproximation& a)
    {
        Eigen::Vector3f v1, v2, v3;
        MathTools::TriangleFace f;

        int nVertices = (int)a.vertices.size();
        int nFaces = (int)a.faces.size();
        VR_ASSERT(nVertices > 0);
        int nVertexCount = 0;

        TriMeshModelPtr tr(new TriMeshModel());

        for (int i = 0; i < nFaces; i++)
        {
            f = a.faces.at(i);
            v1 = a.vertices.at(f.id1);
            v2 = a.vertices.at(f.id2);
            v3 = a.vertices.at(f.id3);
            tr->addTriangleWithFace(v1, v3, v2);
        }

        return tr;
    }
Example #18
0
RobotConfigPtr RobotConfig::clone( RobotPtr newRobot )
{
	if (!newRobot)
		newRobot = robot.lock();
	VR_ASSERT (newRobot);
	
	std::map< RobotNodePtr, float > newConfigs;
	std::map< RobotNodePtr, float >::iterator i = configs.begin();
	while (i!=configs.end())
	{
		RobotNodePtr rn = newRobot->getRobotNode(i->first->getName());
		if (!rn)
		{
			VR_WARNING << "Could not completely clone RobotConfig " << name << " because new robot does not know a RobotNode with name " << i->first->getName() << endl;
		} else
		{
			newConfigs[rn] = i->second;
		}
		i++;
	}
	RobotConfigPtr result(new RobotConfig(newRobot,name,newConfigs));
	return result;
}
Example #19
0
void PathProcessingThread::workingMethod()
{
	if (!threadStarted)
	{
		VR_WARNING << "Thread should be in started mode?!" << endl;
	}
	VR_ASSERT(pathProcessor);

	CSpacePathPtr res = pathProcessor->optimize(optimizeSteps);

	mutex.lock();

	if (res)
		processingFinished = true;
	else
		processingFinished = false;

	resultPath = res;

    // the thread ends here
    threadStarted = false;
	mutex.unlock();
}
Example #20
0
    void MeshConverter::checkAndSplitVertex(VirtualRobot::TriMeshModelPtr tm, int faceIdx, float maxDist)
    {
        VR_ASSERT(tm);
        VR_ASSERT(faceIdx >= 0 && faceIdx < (int)tm->faces.size());

        float d12, d13, d23;
        Eigen::Vector3f v1, v2, v3;
        v1 = tm->vertices[tm->faces[faceIdx].id1];
        v2 = tm->vertices[tm->faces[faceIdx].id2];
        v3 = tm->vertices[tm->faces[faceIdx].id3];
        Eigen::Vector3f v4;
        unsigned int id4;
        size_t checkFaceIdx;

        d12 = (v1 - v2).norm();
        d23 = (v2 - v3).norm();
        d13 = (v1 - v3).norm();

        if (d12 > maxDist || d23 > maxDist || d13 > maxDist)
        {
            // split at longest edge
            if (d12 >= d23 && d12 >= d13)
            {
                v4 = v1 + (v2 - v1) * 0.5f;
                tm->addVertex(v4);
                id4 = tm->vertices.size() - 1;

                // add new face
                MathTools::TriangleFace face;
                face.id1 = id4;
                face.id2 = tm->faces[faceIdx].id2;
                face.id3 = tm->faces[faceIdx].id3;
                face.normal = tm->faces[faceIdx].normal;
                tm->addFace(face);

                // update current face
                tm->faces[faceIdx].id2 = id4;
            }
            else if (d23 >= d12 && d23 >= d13)
            {
                v4 = v2 + (v3 - v2) * 0.5f;
                tm->addVertex(v4);
                id4 = tm->vertices.size() - 1;

                // add new face
                MathTools::TriangleFace face;
                face.id1 = tm->faces[faceIdx].id1;
                face.id2 = id4;
                face.id3 = tm->faces[faceIdx].id3;
                face.normal = tm->faces[faceIdx].normal;
                tm->addFace(face);

                // update current face
                tm->faces[faceIdx].id3 = id4;
            }
            else
            {
                v4 = v3 + (v1 - v3) * 0.5f;
                tm->addVertex(v4);
                id4 = tm->vertices.size() - 1;

                // add new face
                MathTools::TriangleFace face;
                face.id1 = tm->faces[faceIdx].id1;
                face.id2 = tm->faces[faceIdx].id2;
                face.id3 = id4;
                face.normal = tm->faces[faceIdx].normal;
                tm->addFace(face);

                // update current face
                tm->faces[faceIdx].id1 = id4;

            }

            checkFaceIdx = tm->faces.size() - 1;
            // check new face
            checkAndSplitVertex(tm, checkFaceIdx, maxDist);
            // recursively check current face
            checkAndSplitVertex(tm, faceIdx, maxDist);
        }
    }
Example #21
0
void DynamicsRobot::actuateNode(const std::string &node, float jointValue )
{
    VR_ASSERT(robot);
    VR_ASSERT(robot->hasRobotNode(node));
    actuateNode(robot->getRobotNode(node),jointValue);
}
Example #22
0
void Trajectory::addPoint(const Eigen::VectorXf &c)
{
	VR_ASSERT (c.rows() == dimension);
	path.push_back(c);
}
Example #23
0
float PoseQualityMeasurement::getPoseQuality( const Eigen::VectorXf &direction )
{
	VR_ASSERT(direction.rows()==3 || direction.rows()==6);
	VR_WARNING << "Please use derived classes..." << endl;
	return 0.0f;
}