void CommandTourner::executer(){
old_direction = getRobot()->getDirection();
getRobot()->tourner(direction);
Command::historique().push(this);
}
void Tendon::removeWrapperIntersections()
{
int j;
SbVec3f pPrev,pCur,pNext;
position tmpPos;
vec3 dPrev,dRes,P3,P4,Pa,Pb;
double mua,mub;
Link *link;
bool needed;
std::list<TendonInsertionPoint*>::iterator prevInsPt, insPt, nextInsPt;
for (insPt=insPointList.begin(); insPt!=insPointList.end(); insPt++)
{
nextInsPt = insPt;
prevInsPt = insPt;
nextInsPt ++;
if ( !(*insPt)->isPermanent() && insPt!=insPointList.begin() && nextInsPt!=insPointList.end() )
{
prevInsPt--;
pPrev = (*prevInsPt)->getWorldPosition();
pCur = (*insPt)->getWorldPosition();
pNext = (*nextInsPt)->getWorldPosition();
needed = false;
for (j=0; j< ((HumanHand*)getRobot())->nrTendonWrappers; j++)
{
//two points along axis of tendon wrapper
P3 = ((HumanHand*)getRobot())->getTendonWrapper(j)->location;
P4 = P3 + ((HumanHand*)getRobot())->getTendonWrapper(j)->orientation;
//convert them to world coordinates
link = ((HumanHand*)getRobot())->getTendonWrapper(j)->getAttachedLink();
tmpPos = position (P3.toSbVec3f()) * ( link->getTran());
P3 = vec3 ( tmpPos.toSbVec3f() );
tmpPos = position (P4.toSbVec3f()) * ( link->getTran());
P4 = vec3 ( tmpPos.toSbVec3f() );
LineLineIntersect( vec3(pPrev) , vec3(pNext) , P3,P4 , &Pa, &Pb, &mua, &mub);
dPrev = Pa - Pb;
/*careful: here we are using the exact radius, not a slightly smaller value*/
/*changed my mind: we are*/
if (dPrev.len() < WRAPPER_TOLERANCE * ((HumanHand*)getRobot())->getTendonWrapper(j)->radius &&
mua>0 && mua<1)
{
needed = true;
break;
}
}
if (!needed)
{
removeInsertionPoint(insPt);
insPt = prevInsPt;
}
}
}
}
/*! Checks if a connector penetrates a cylindrical wrapper by more than the
tolerance value. If so, it adds a temporary insertion point on the edge
of the cylider. Problems:
- creates a small "jump" in the connector as it goes from being INSIDE
the wrapper (acc. to tolerance) to passing through a point ON THE EDGE
of the wrapper. This might create discontinuities between time steps
for dynamics' numerical integration
- does not allow for lateral "sliding" of the tendon on the wrapper
*/
void Tendon::checkWrapperIntersections()
{
int j;
int chainNr,linkNr;
SbVec3f pCur,pNext;
position tmpPos;
vec3 dPrev,dRes,P3,P4,Pa,Pb;
double mua,mub;
Link *link;
std::list<TendonInsertionPoint*>::iterator prevInsPt, insPt, nextInsPt, newInsPt;
for (insPt=insPointList.begin(); insPt!=insPointList.end(); insPt++) {
nextInsPt = insPt;
nextInsPt ++;
pCur = (*insPt)->getWorldPosition();
if (nextInsPt != insPointList.end() ) {
pNext = (*nextInsPt)->getWorldPosition();
for (j=0; j< ((HumanHand*)getRobot())->nrTendonWrappers; j++) {
//two points along axis of tendon wrapper
P3 = ((HumanHand*)getRobot())->getTendonWrapper(j)->location;
P4 = P3 + ((HumanHand*)getRobot())->getTendonWrapper(j)->orientation;
//convert them to world coordinates
link = ((HumanHand*)getRobot())->getTendonWrapper(j)->getAttachedLink();
tmpPos = position (P3.toSbVec3f()) * ( link->getTran());
P3 = vec3 ( tmpPos.toSbVec3f() );
tmpPos = position (P4.toSbVec3f()) * ( link->getTran());
P4 = vec3 ( tmpPos.toSbVec3f() );
LineLineIntersect( vec3(pCur) , vec3(pNext) , P3,P4 , &Pa, &Pb, &mua, &mub);
// check two things:
//- if tendon is too close to wrapper
//- if closest point actually falls between insertion points
// (wrappers extends to infinity, we don't check that)
//- WE SHOULD IN THE FUTURE! don't want one finger's tendon to wrap around another finger's wrapper
//
dPrev = Pa - Pb;
if (dPrev.len() < WRAPPER_TOLERANCE * ((HumanHand*)getRobot())->getTendonWrapper(j)->radius
&& mua>0 && mua<1)
{
/* compute location of new insertion point - on cylinder edge */
dPrev = normalise(dPrev);
dRes = Pb + ( ((HumanHand*)getRobot())->getTendonWrapper(j)->radius ) * dPrev;
/* transform it to coordinate system of wrapper */
tmpPos = position ( dRes.toSbVec3f() ) * ( link->getTran().inverse() );
chainNr = ((HumanHand*)getRobot())->getTendonWrapper(j)->getChainNr();
linkNr = ((HumanHand*)getRobot())->getTendonWrapper(j)->getLinkNr();
/*create new insertion point*/
newInsPt = insertInsertionPoint( nextInsPt, chainNr, linkNr, vec3(tmpPos.toSbVec3f()), false );
}
}
}
}
}
void RobotNodeRevolute::print(bool printChildren, bool printDecoration) const
{
ReadLockPtr lock = getRobot()->getReadLock();
if (printDecoration)
{
cout << "******** RobotNodeRevolute ********" << endl;
}
RobotNode::print(false, false);
cout << "* JointRotationAxis: " << jointRotationAxis[0] << ", " << jointRotationAxis[1] << ", " << jointRotationAxis[2] << endl;
if (printDecoration)
{
cout << "******** End RobotNodeRevolute ********" << endl;
}
std::vector< SceneObjectPtr > children = this->getChildren();
if (printChildren)
{
std::for_each(children.begin(), children.end(), boost::bind(&SceneObject::print, _1, true, true));
}
}
void RobotNode::setJointValue(float q)
{
RobotPtr r = getRobot();
VR_ASSERT(r);
WriteLockPtr lock = r->getWriteLock();
setJointValueNoUpdate(q);
updatePose();
}
bool RobotNode::checkJointLimits( float jointValue, bool verbose ) const
{
ReadLockPtr lock = getRobot()->getReadLock();
bool res = true;
if (jointValue < jointLimitLo)
res = false;
if (jointValue > jointLimitHi)
res = false;
if (!res && verbose)
VR_INFO << "Joint: " << getName() << ": joint value (" << jointValue << ") is out of joint boundaries (lo:" << jointLimitLo << ", hi: " << jointLimitHi <<")" << endl;
return res;
}
void RobotNode::updateTransformationMatrices()
{
if (this->getParent())
updateTransformationMatrices(this->getParent()->getGlobalPose());
else
{
// check for root
RobotPtr r = getRobot();
if (r && r->getRootNode() == shared_from_this())
updateTransformationMatrices(r->getGlobalPose());
else
updateTransformationMatrices(Eigen::Matrix4f::Identity());
}
}
void MapWidget::tfChanged(const std::string& robot_id, const QPointF& position, double heading) {
boost::mutex::scoped_lock lock(lock_);
RobotGraphicsItem* robot = getRobot(robot_id);
if (robot == NULL) {
robot = registrateRobot(robot_id, position);
if (robot == NULL) {
ROS_WARN("Robot registration failed. Key was present");
return;
}
}
robot->updatePosition(position, heading);
}
Eigen::Vector3f RobotNodeRevolute::getJointRotationAxis(const SceneObjectPtr coordSystem) const
{
ReadLockPtr lock = getRobot()->getReadLock();
Eigen::Vector4f result4f = Eigen::Vector4f::Zero();
result4f.segment(0, 3) = jointRotationAxis;
result4f = globalPose * result4f;
if (coordSystem)
{
//res = coordSystem->toLocalCoordinateSystem(res);
result4f = coordSystem->getGlobalPose().inverse() * result4f;
}
return result4f.block(0, 0, 3, 1);
}
Eigen::Vector3f RobotNodePrismatic::getJointTranslationDirection(const SceneObjectPtr coordSystem) const
{
ReadLockPtr lock = getRobot()->getReadLock();
Eigen::Vector4f result4f = Eigen::Vector4f::Zero();
result4f.segment(0, 3) = jointTranslationDirection;
result4f = getGlobalPose() * result4f;
if (coordSystem)
{
result4f = coordSystem->getGlobalPose().inverse() * result4f;
}
return result4f.segment(0, 3);
}
RobotNodePtr RobotNodeFixed::_clone(const RobotPtr newRobot, const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker, float scaling)
{
ReadLockPtr lock = getRobot()->getReadLock();
RobotNodePtr result;
Physics p = physics.scale(scaling);
if (optionalDHParameter.isSet)
{
result.reset(new RobotNodeFixed(newRobot,name,optionalDHParameter.aMM()*scaling,optionalDHParameter.dMM()*scaling, optionalDHParameter.alphaRadian(), optionalDHParameter.thetaRadian(),visualizationModel,collisionModel,p,colChecker,nodeType));
} else
{
Eigen::Matrix4f lt = getLocalTransformation();
lt.block(0,3,3,1) *= scaling;
result.reset(new RobotNodeFixed(newRobot,name,lt,visualizationModel,collisionModel,p,colChecker,nodeType));
}
return result;
}
void RobotNode::updateVisualizationPose( const Eigen::Matrix4f &globalPose, bool updateChildren )
{
// check if we are a root node
SceneObjectPtr parent = getParent();
RobotPtr rob = getRobot();
if (!parent || parent == rob)
{
if (rob && rob->getRootNode() == static_pointer_cast<RobotNode>(shared_from_this()))
{
Eigen::Matrix4f gpPre = getLocalTransformation().inverse() * globalPose;
rob->setGlobalPose(gpPre, false);
} else
VR_WARNING << "INTERNAL ERROR: getParent==robot but getRoot!=this ?! " << endl;
}
this->globalPose = globalPose;
// update collision and visualization model and children
SceneObject::updatePose(updateChildren);
}
RobotGraphicsItem* MapWidget::registrateRobot(const std::string& robot_id,
const QPointF& position) {
boost::mutex::scoped_lock lock = boost::mutex::scoped_lock(registration_lock_);
RobotGraphicsItem* robot = getRobot(robot_id);
if (robot != NULL)
return robot;
robot = new RobotGraphicsItem(robot_id, position, 0, scene_);
connect(robot, SIGNAL(selected(const std::string&)),
this, SLOT(selected(const std::string&)));
robot_pair_t entry = std::make_pair<std::string, RobotGraphicsItem*>(robot_id, robot);
if (!robots_.insert(entry).second) {
return NULL;
}
ROS_INFO("Robot '%s' has been registered", robot_id.c_str());
return robot;
}
// ---------------------------------------------------------------------------
// SimulatorCL::reset
// ---------------------------------------------------------------------------
void SimulatorCL::reset()
{
// envoie l'ordre de reset aux robots
if (server_) {
for(unsigned int i=0;i<SIMU_PORT_MAX_CONNECTIONS; i++) {
SimulatorRobot* robot = getRobot(i);
if (robot) {
robot->setMatchStatusReset();
}
}
}
// pont
Viewer3D->setBridgePosition(bridge_);
// balles de grs
gBalls_[0].center = Point(1522, 1050+75);
gBalls_[1].center = Point(TERRAIN_X-1522, 1050-75);
Viewer3D->setGRSBallsPosition(gBalls_);
// balles de squatch
Viewer3D->setSquatchBallsPosition(sBalls_);
}
void MapWidget::setViewOnRobot(std::string robot_id)
{
if (active_view_on_robot_ == robot_id)
{
return;
}
RobotGraphicsItem* robot = getRobot(robot_id);
QPointF position;
double heading;
getRobotPosition("/"+robot_id+"/base_link", position, heading);
graphicsView_->setTransform(QTransform());
graphicsView_->centerOn(position.x(),position.y());
// graphicsView_->scale(1, -1);
graphicsView_->scale(56, -56);
active_view_on_robot_ = robot_id;
}
void CPairTrade::helperThreadFun()
{
threadStarted_m.signalEvent();
LOG(MSG_INFO, PAIR_TRADE, CPairTrade::getName() << " - helper thread started");
LOG_COMMON(connectRunName_m, getBarDataProvider_Smartcom(), BrkLib::AP_MEDIUM, "Helper thread started");
while(true) {
// lock
bool res = shutDownThread_m.lock(1000);
if (res)
break;
try {
getRobot().getLevelCalculator().heartBeat();
}
catch(CppUtils::Exception& e) {
LOG_COMMON(connectRunName_m, getBarDataProvider_Smartcom(), BrkLib::AP_MEDIUM, "Error in helper thread: " << e.message());
}
#ifdef HANDLE_NATIVE_EXCEPTIONS
catch(Testframe::Exception& e) {
LOG_COMMON(connectRunName_m, getBarDataProvider_Smartcom(), BrkLib::AP_MEDIUM, "MSVC error in helper thread: " << e.message());
}
#endif
catch(...) {
LOG_COMMON(connectRunName_m, getBarDataProvider_Smartcom(), BrkLib::AP_MEDIUM, "Unknown error in helper thread");
}
}
LOG_COMMON(connectRunName_m, getBarDataProvider_Smartcom(), BrkLib::AP_MEDIUM, "Helper thread finished");
LOG(MSG_INFO, PAIR_TRADE, CPairTrade::getName() << " - helper thread finished");
}
RobotNodePtr RobotNodePrismatic::_clone(const RobotPtr newRobot, const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker, float scaling)
{
boost::shared_ptr<RobotNodePrismatic> result;
ReadLockPtr lock = getRobot()->getReadLock();
Physics p = physics.scale(scaling);
if (optionalDHParameter.isSet)
{
result.reset(new RobotNodePrismatic(newRobot, name, jointLimitLo, jointLimitHi, optionalDHParameter.aMM()*scaling, optionalDHParameter.dMM()*scaling, optionalDHParameter.alphaRadian(), optionalDHParameter.thetaRadian(), visualizationModel, collisionModel, jointValueOffset, p, colChecker, nodeType));
}
else
{
Eigen::Matrix4f lt = getLocalTransformation();
lt.block(0, 3, 3, 1) *= scaling;
result.reset(new RobotNodePrismatic(newRobot, name, jointLimitLo, jointLimitHi, lt, jointTranslationDirection, visualizationModel, collisionModel, jointValueOffset, p, colChecker, nodeType));
}
if (result && visuScaling)
{
result->setVisuScaleFactor(visuScaleFactor);
}
return result;
}
void RobotNodePrismatic::print(bool printChildren, bool printDecoration) const
{
ReadLockPtr lock = getRobot()->getReadLock();
if (printDecoration)
{
cout << "******** RobotNodePrismatic ********" << endl;
}
RobotNode::print(false, false);
cout << "* jointTranslationDirection: " << jointTranslationDirection[0] << ", " << jointTranslationDirection[1] << "," << jointTranslationDirection[2] << endl;
cout << "* VisuScaling: ";
if (visuScaling)
{
cout << visuScaleFactor[0] << ", " << visuScaleFactor[1] << "," << visuScaleFactor[2] << endl;
}
else
{
cout << "disabled" << endl;
}
if (printDecoration)
{
cout << "******** End RobotNodePrismatic ********" << endl;
}
std::vector< SceneObjectPtr > children = this->getChildren();
if (printChildren)
{
std::for_each(children.begin(), children.end(), boost::bind(&SceneObject::print, _1, true, true));
}
}
RobotNodePtr RobotNode::clone( RobotPtr newRobot, bool cloneChildren, RobotNodePtr initializeWithParent, CollisionCheckerPtr colChecker, float scaling )
{
ReadLockPtr lock = getRobot()->getReadLock();
if (!newRobot)
{
VR_ERROR << "Attempting to clone RobotNode for invalid robot";
return RobotNodePtr();
}
std::vector< std::string > clonedChildrenNames;
VisualizationNodePtr clonedVisualizationNode;
if (visualizationModel)
clonedVisualizationNode = visualizationModel->clone(true,scaling);
CollisionModelPtr clonedCollisionModel;
if (collisionModel)
clonedCollisionModel = collisionModel->clone(colChecker,scaling);
RobotNodePtr result = _clone(newRobot, clonedVisualizationNode, clonedCollisionModel, colChecker, scaling);
if (!result)
{
VR_ERROR << "Cloning failed.." << endl;
return result;
}
if (cloneChildren)
{
std::vector< SceneObjectPtr > children = this->getChildren();
for (size_t i=0;i<children.size();i++)
{
RobotNodePtr n = dynamic_pointer_cast<RobotNode>(children[i]);
if (n)
{
RobotNodePtr c = n->clone(newRobot,true,RobotNodePtr(),colChecker,scaling);
if (c)
result->attachChild(c);
} else
{
SensorPtr s = dynamic_pointer_cast<Sensor>(children[i]);
if (s)
{
// performs registering and initialization
SensorPtr c = s->clone(result,scaling);
} else
{
SceneObjectPtr so = children[i]->clone(children[i]->getName(),colChecker,scaling);
if (so)
result->attachChild(so);
}
}
}
}
result->setMaxVelocity(maxVelocity);
result->setMaxAcceleration(maxAcceleration);
result->setMaxTorque(maxTorque);
std::map< std::string, float>::iterator it = propagatedJointValues.begin();
while (it != propagatedJointValues.end())
{
result->propagateJointValue(it->first,it->second);
it++;
}
newRobot->registerRobotNode(result);
if (initializeWithParent)
result->initialize(initializeWithParent);
return result;
}
OpticalSensors& Main::getOpticalSensors()
{
return getRobot().m_optical;
}
GyroSensor& Main::getGyroSensor()
{
return getRobot().m_gyro;
}
Flashlight& Main::getFlashlight()
{
return getRobot().m_flashlight;
}
UltrasonicSensor& Main::getUltrasonicSensor()
{
return getRobot().m_ultrasonic;
}
void CommandPoser::executer(){
getRobot()->poser();
Command::historique().push(this);
}
/* This is called at each goal point. It moves the robot forward a bit, waits 3 seconds
* then backs the mobile robot up. Then it tells the ARNL path planner to
* navigate to the next goal point in the chain (named Goal 0, Goal 1, Goal 2, etc.)
* The last goal point is special, no action and the chain of ARNL goals
* stops. The robot will wait at this goal point until sent to the first
* goal manually from MobileEyes.
*/
void runTask()
{
// Make copies of variables shared between threads
lock();
int currentGoal = myCurrentGoal;
int numGoals = myNumGoals;
int moveDist = myApproachDist;
unlock();
// if at end of chain, stop chain
if(currentGoal == numGoals)
{
ArLog::log(ArLog::Normal, "Waiting to be loaded, end of chain.");
if(myServerMode) myServerMode->setStatus("End of goal chain.");
lock();
myCurrentGoal = 1;
unlock();
return; // end of thread
}
/* In this example, we will move the robot forward a bit, wait, then move it
back.
In your application, you could do somethng here like trigger devices, cameras,
sensors, output, speech, etc. You can access the current goal name and
use that to control the activity, or use that goal name to obtain the
goal object from the map ArMap object which has additional properties of
the goal.
*/
// move forward a bit
ArLog::log(ArLog::Normal, "Moving forward a bit");
if(myServerMode) myServerMode->setStatus("Moving forward");
getRobot()->clearDirectMotion();
getRobot()->move(moveDist);
waitForMoveDone();
getRobot()->clearDirectMotion();
ArUtil::sleep(500);
// Wait a bit.
ArLog::log(ArLog::Normal, "Would do goal-specific task at goal %d.", currentGoal);
ArLog::log(ArLog::Normal, "Waiting 3 sec.");
if(myServerMode) myServerMode->setStatus("Waiting 3 sec");
ArUtil::sleep(3000);
// back up a bit
ArLog::log(ArLog::Normal, "Backing up a bit");
if(myServerMode) myServerMode->setStatus("Backing up a bit");
getRobot()->clearDirectMotion();
getRobot()->move(-moveDist);
waitForMoveDone();
getRobot()->clearDirectMotion();
ArUtil::sleep(500);
/* Go to the next goal in the chain. The name is assumed to be "Goal X"
where X is the goal index number. You could use another scheme for
naming goals, or you could store a list of strings in this class
*/
++currentGoal;
if(currentGoal > numGoals) currentGoal = 0;
char name[128];
snprintf(name, 127, "Goal %d", currentGoal);
ArLog::log(ArLog::Normal, "Going to next goal %s", name);
if(myServerMode) myServerMode->setStatus("ASyncTask example done. Going to next goal.");
nextGoal(name);
// Save the new goal index
lock();
myCurrentGoal = currentGoal;
unlock();
// This is the end of the thread.
return;
}
Eigen::Matrix4f RobotNode::getGlobalPose() const
{
ReadLockPtr lock = getRobot()->getReadLock();
return globalPose;
}
float RobotNode::getJointValue() const
{
ReadLockPtr lock = getRobot()->getReadLock();
return jointValue;
}
void RobotNode::showStructure( bool enable, const std::string &visualizationType)
{
ReadLockPtr lock = getRobot()->getReadLock();
if (!enable && !visualizationModel)
return; // nothing to do
if (!ensureVisualization(visualizationType))
return;
std::stringstream ss;
ss << getName() << "_RobotNodeStructurePre";
std::string attachName1 = ss.str();
std::string attachName2("RobotNodeStructureJoint");
std::string attachName3("RobotNodeStructurePost");
SceneObjectPtr par = getParent();
RobotNodePtr parRN = dynamic_pointer_cast<RobotNode>(par);
// need to add "pre" visualization to parent node!
if (parRN && parRN->getVisualization())
parRN->getVisualization()->detachVisualization(attachName1);
else
visualizationModel->detachVisualization(attachName1);
visualizationModel->detachVisualization(attachName2);
visualizationModel->detachVisualization(attachName3);
if (enable)
{
VisualizationFactoryPtr visualizationFactory;
if (visualizationType.empty())
visualizationFactory = VisualizationFactory::first(NULL);
else
visualizationFactory = VisualizationFactory::fromName(visualizationType, NULL);
if (!visualizationFactory)
{
VR_WARNING << "No visualization factory for name " << visualizationType << endl;
return;
}
// create visu
Eigen::Matrix4f i = Eigen::Matrix4f::Identity();
if (!localTransformation.isIdentity())
{
VisualizationNodePtr visualizationNode1;
if (parRN && parRN->getVisualization())
{
// add to parent node (pre joint trafo moves with parent!)
//visualizationNode1 = visualizationFactory->createLine(parRN->postJointTransformation, parRN->postJointTransformation*localTransformation);
visualizationNode1 = visualizationFactory->createLine(Eigen::Matrix4f::Identity(), localTransformation);
if (visualizationNode1)
parRN->getVisualization()->attachVisualization(attachName1,visualizationNode1);
} else
{
visualizationNode1 = visualizationFactory->createLine(localTransformation.inverse(),i);
if (visualizationNode1)
visualizationModel->attachVisualization(attachName1,visualizationNode1);
}
}
VisualizationNodePtr visualizationNode2 = visualizationFactory->createSphere(5.0f);
if (visualizationNode2)
visualizationModel->attachVisualization(attachName2,visualizationNode2);
}
}
float RobotNode::getJointLimitHi()
{
ReadLockPtr lock = getRobot()->getReadLock();
return jointLimitHi;
}
void RobotNode::print( bool printChildren, bool printDecoration ) const
{
ReadLockPtr lock = getRobot()->getReadLock();
if (printDecoration)
cout << "******** RobotNode ********" << endl;
cout << "* Name: " << name << endl;
cout << "* Parent: ";
SceneObjectPtr p = this->getParent();
if (p)
cout << p->getName() << endl;
else
cout << " -- " << endl;
cout << "* Children: ";
if (this->getChildren().size() == 0)
cout << " -- " << endl;
for (unsigned int i = 0; i < this->getChildren().size(); i++)
cout << this->getChildren()[i]->getName() << ", ";
cout << endl;
physics.print();
cout << "* Limits: Lo:" << jointLimitLo << ", Hi:" << jointLimitHi << endl;
std::cout << "* max velocity " << maxVelocity << " [m/s]" << std::endl;
std::cout << "* max acceleration " << maxAcceleration << " [m/s^2]" << std::endl;
std::cout << "* max torque " << maxTorque << " [Nm]" << std::endl;
cout << "* jointValue: " << this->getJointValue() << ", jointValueOffset: " << jointValueOffset << endl;
if (optionalDHParameter.isSet)
{
cout << "* DH parameters: ";
cout << " a:" << optionalDHParameter.aMM() << ", d:" << optionalDHParameter.dMM() << ", alpha:" << optionalDHParameter.alphaRadian() << ", theta:" << optionalDHParameter.thetaRadian() << endl;
} else
cout << "* DH parameters: not specified." << endl;
cout << "* visualization model: " <<endl;
if (visualizationModel)
visualizationModel->print();
else
cout << " No visualization model" << endl;
cout << "* collision model: " << endl;
if (collisionModel)
collisionModel->print();
else
cout << " No collision model" << endl;
if (initialized)
cout << "* initialized: true" << endl;
else
cout << "* initialized: false" << endl;
{ // scope1
std::ostringstream sos;
sos << std::setiosflags(std::ios::fixed);
sos << "* localTransformation:" << endl << localTransformation << endl;
sos << "* globalPose:" << endl << getGlobalPose() << endl;
cout << sos.str();
} // scope1
if (printDecoration)
cout << "******** End RobotNode ********" << endl;
if (printChildren)
{
std::vector< SceneObjectPtr > children = this->getChildren();
for (unsigned int i = 0; i < children.size(); i++)
children[i]->print(true, true);
}
}
