コード例 #1
0
ファイル: RobotNode.cpp プロジェクト: TheMarex/simox
bool RobotNode::initialize(SceneObjectPtr parent, const std::vector<SceneObjectPtr> &children)
{
	RobotPtr rob = robot.lock();
	THROW_VR_EXCEPTION_IF(!rob, "Could not init RobotNode without robot" );

	// robot
	if (!rob->hasRobotNode(static_pointer_cast<RobotNode>(shared_from_this())))
		rob->registerRobotNode(static_pointer_cast<RobotNode>(shared_from_this()));

	// update visualization of coordinate systems
	if (visualizationModel && visualizationModel->hasAttachedVisualization("CoordinateSystem"))
	{
		VisualizationNodePtr v = visualizationModel->getAttachedVisualization("CoordinateSystem");
		// not needed any more!
		// this is a little hack: The globalPose is used to set the "local" position of the attached Visualization:
		// Since the attached visualizations are already positioned at the global pose of the visualizationModel, 
		// we just need the local postJointTransform
		//v->setGlobalPose(postJointTransformation);
	}

    checkValidRobotNodeType();

	for (size_t i=0;i<sensors.size();i++)
	{
		sensors[i]->initialize(shared_from_this());
	}

	return SceneObject::initialize(parent,children);	
}
コード例 #2
0
ファイル: RobotNodeFixed.cpp プロジェクト: TheMarex/simox
RobotNodeFixed::RobotNodeFixed(RobotWeakPtr rob, 
	const std::string &name,
	const Eigen::Matrix4f &preJointTransform,
	VisualizationNodePtr visualization, 
	CollisionModelPtr collisionModel,
	const SceneObject::Physics &p,
	CollisionCheckerPtr colChecker,
    RobotNodeType type
	) : RobotNode(rob,name,0.0f,0.0f,visualization,collisionModel,0.0f,p,colChecker,type)
{
	optionalDHParameter.isSet = false;
	this->localTransformation = preJointTransform;
    checkValidRobotNodeType();
}
コード例 #3
0
    RobotNodePrismatic::RobotNodePrismatic(RobotWeakPtr rob,
                                           const std::string& name,
                                           float jointLimitLo,
                                           float jointLimitHi,
                                           float a, float d, float alpha, float theta,
                                           VisualizationNodePtr visualization,
                                           CollisionModelPtr collisionModel,
                                           float jointValueOffset,
                                           const SceneObject::Physics& p,
                                           CollisionCheckerPtr colChecker,
                                           RobotNodeType type
                                          ) : RobotNode(rob, name, jointLimitLo, jointLimitHi, visualization, collisionModel, jointValueOffset, p, colChecker, type)
    {
        initialized = false;
        optionalDHParameter.isSet = true;
        optionalDHParameter.setAInMM(a);
        optionalDHParameter.setDInMM(d);
        optionalDHParameter.setAlphaRadian(alpha, true);
        optionalDHParameter.setThetaRadian(theta, true);

        // compute DH transformation matrices
        Eigen::Matrix4f RotTheta = Eigen::Matrix4f::Identity();
        RotTheta(0, 0) = cos(theta);
        RotTheta(0, 1) = -sin(theta);
        RotTheta(1, 0) = sin(theta);
        RotTheta(1, 1) = cos(theta);
        Eigen::Matrix4f TransD = Eigen::Matrix4f::Identity();
        TransD(2, 3) = d;
        Eigen::Matrix4f TransA = Eigen::Matrix4f::Identity();
        TransA(0, 3) = a;
        Eigen::Matrix4f RotAlpha = Eigen::Matrix4f::Identity();
        RotAlpha(1, 1) = cos(alpha);
        RotAlpha(1, 2) = -sin(alpha);
        RotAlpha(2, 1) = sin(alpha);
        RotAlpha(2, 2) = cos(alpha);

        // fixed rotation around theta
        this->localTransformation = RotTheta * TransD * TransA * RotAlpha;
        // joint setup
        jointTranslationDirection = Eigen::Vector3f(0, 0, 1); // translation along the z axis
        checkValidRobotNodeType();
    }
コード例 #4
0
 RobotNodeRevolute::RobotNodeRevolute(RobotWeakPtr rob,
                                      const std::string& name,
                                      float jointLimitLo,
                                      float jointLimitHi,
                                      const Eigen::Matrix4f& preJointTransform,            //!< This transformation is applied before the translation of the joint is done
                                      const Eigen::Vector3f& axis,         //!< This is the direction of the translation
                                      VisualizationNodePtr visualization,
                                      CollisionModelPtr collisionModel,
                                      float jointValueOffset,
                                      const SceneObject::Physics& p,
                                      CollisionCheckerPtr colChecker,
                                      RobotNodeType type
                                     ) : RobotNode(rob, name, jointLimitLo, jointLimitHi, visualization, collisionModel, jointValueOffset, p, colChecker, type)
 {
     initialized = false;
     optionalDHParameter.isSet = false;
     this->localTransformation = preJointTransform;
     this->jointRotationAxis = axis;
     checkValidRobotNodeType();
 }
コード例 #5
0
 RobotNodePrismatic::RobotNodePrismatic(RobotWeakPtr rob,
                                        const std::string& name,
                                        float jointLimitLo,
                                        float jointLimitHi,
                                        const Eigen::Matrix4f& preJointTransform,
                                        const Eigen::Vector3f& translationDirection,
                                        VisualizationNodePtr visualization,
                                        CollisionModelPtr collisionModel,
                                        float jointValueOffset,
                                        const SceneObject::Physics& p,
                                        CollisionCheckerPtr colChecker,
                                        RobotNodeType type
                                       ) : RobotNode(rob, name, jointLimitLo, jointLimitHi, visualization, collisionModel, jointValueOffset, p, colChecker, type)
 {
     initialized = false;
     visuScaling = false;
     visuScaleFactor << 1.0f, 1.0f, 1.0f;
     optionalDHParameter.isSet = false;
     this->localTransformation = preJointTransform;
     this->jointTranslationDirection = translationDirection;
     checkValidRobotNodeType();
 }