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