void RobotNode::updatePose(bool updateChildren)
{
THROW_VR_EXCEPTION_IF(!initialized, "Not initialized");
updateTransformationMatrices();
// update collision and visualization model and children
SceneObject::updatePose(updateChildren);
// apply propagated joint values
if (propagatedJointValues.size()>0)
{
RobotPtr r = robot.lock();
std::map< std::string, float>::iterator it = propagatedJointValues.begin();
while (it!=propagatedJointValues.end())
{
RobotNodePtr rn = r->getRobotNode(it->first);
if (!rn)
{
VR_WARNING << "Could not propagate joint value from " << name << " to " << it->first << " because dependent joint does not exist...";
} else
{
rn->setJointValue(jointValue*it->second);
}
it++;
}
}
}
SimoxMotionState::SimoxMotionState( VirtualRobot::SceneObjectPtr sceneObject)
:btDefaultMotionState()
{
this->sceneObject = sceneObject;
initalGlobalPose.setIdentity();
if (sceneObject)
{
initalGlobalPose = sceneObject->getGlobalPoseVisualization();
}
_transform.setIdentity();
_graphicsTransfrom.setIdentity();
_comOffset.setIdentity();
Eigen::Vector3f com = sceneObject->getCoMLocal();
RobotNodePtr rn = boost::dynamic_pointer_cast<RobotNode>(sceneObject);
if (rn)
{
// we are operating on a RobotNode, so we need a RobtoNodeActuator to move it later on
robotNodeActuator.reset(new RobotNodeActuator(rn));
// localcom is given in coord sytsem of robotnode (== endpoint of internal transformation)
// we need com in visualization coord system (== without postjointtransform)
Eigen::Matrix4f t;
t.setIdentity();
t.block(0,3,3,1)=rn->getCoMGlobal();
t = rn->getGlobalPoseVisualization().inverse() * t;
com = t.block(0,3,3,1);
}
_setCOM(com);
setGlobalPose(initalGlobalPose);
}
bool RobotConfig::setJointValues( RobotPtr r )
{
if (!r)
return false;
WriteLockPtr lock = r->getWriteLock();
std::map < std::string, float > jv = getRobotNodeJointValueMap();
std::map< std::string, float >::const_iterator i = jv.begin();
// first check if all nodes are present
while (i != jv.end())
{
if (!r->hasRobotNode(i->first))
return false;
i++;
}
// apply jv
i = jv.begin();
while (i != jv.end())
{
RobotNodePtr rn = r->getRobotNode(i->first);
if (!rn)
return false;
rn->setJointValueNoUpdate(i->second);
i++;
}
r->applyJointValues();
return true;
}
bool SimoxRobotViewer::setJointLimits( const std::string &robotNodeSet, std::vector<float> &min, std::vector<float> &max )
{
if (!robot)
{
VR_ERROR << "No robot..." << endl;
return false;
}
lock();
RobotNodeSetPtr rns = robot->getRobotNodeSet(robotNodeSet);
if (!rns)
{
VR_ERROR << "No robot node set with name " << robotNodeSet << endl;
unlock();
return false;
}
if (rns->getSize()!=min.size() || rns->getSize()!=max.size())
{
VR_ERROR << "Wrong sizes. RNS (" << robotNodeSet << ") : " << rns->getSize() <<", minSize:" << min.size() << ", maxSize:" << max.size() << endl;
unlock();
return false;
}
for (size_t i=0;i<min.size();i++)
{
RobotNodePtr rn = rns->getNode(i);
if (!rn)
return false;
rn->setJointLimits(min[i],max[i]);
}
unlock();
return true;
}
void RobotNode::collectAllRobotNodes( std::vector< RobotNodePtr > &storeNodes )
{
storeNodes.push_back(static_pointer_cast<RobotNode>(shared_from_this()));
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)
n->collectAllRobotNodes(storeNodes);
}
}
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();
}
void GenericIKSolver::setJointsRandom()
{
std::vector<float> jv;
float rn = 1.0f / (float)RAND_MAX;
for (unsigned int i=0; i<rns->getSize(); i++)
{
RobotNodePtr ro = rns->getNode(i);
float r = (float)rand() * rn;
float v = ro->getJointLimitLo() + (ro->getJointLimitHi() - ro->getJointLimitLo()) * r;
jv.push_back(v);
}
RobotPtr rob = rns->getRobot();
rob->setJointValues(rns,jv);
if (translationalJoint)
{
translationalJoint->setJointValue(initialTranslationalJointValue);
}
}
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;
}
Eigen::Matrix4f SimoxRobotViewer::getObjectPose( const std::string &objectName, const std::string coordSystem )
{
lock();
if (objects.find(objectName) == objects.end())
{
VR_INFO << "Do not know object " << objectName << endl;
unlock();
return Eigen::Matrix4f::Identity();
}
Eigen::Matrix4f r = objects[objectName].object->getGlobalPose();
if (!coordSystem.empty() && robot && robot->hasRobotNode(coordSystem))
{
RobotNodePtr rn = robot->getRobotNode(coordSystem);
if (rn)
r = rn->toLocalCoordinateSystem(r);
}
unlock();
return r;
}
void IKRRTWindow::showCoordSystem()
{
if (eef)
{
RobotNodePtr tcp = eef->getTcp();
if (!tcp)
{
return;
}
tcp->showCoordinateSystem(UI.checkBoxTCP->isChecked());
}
if (object)
{
object->showCoordinateSystem(UI.checkBoxTCP->isChecked());
}
}
bool SimoxRobotViewer::setJointLimit( const std::string &robotNode, float min, float max )
{
if (!robot)
{
VR_ERROR << "No robot..." << endl;
return false;
}
lock();
RobotNodePtr rn = robot->getRobotNode(robotNode);
if (!rn)
{
VR_ERROR << "No robot node with name " << robotNode << endl;
unlock();
return false;
}
rn->setJointLimits(min,max);
unlock();
return true;
}
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
}
bool SimoxRobotViewer::setObjectPose( const std::string &objectName, const Eigen::Matrix4f &pose, const std::string coordSystem )
{
if (!sceneSep)
return false;
lock();
Eigen::Matrix4f p = pose;
if (objects.find(objectName) == objects.end())
{
VR_INFO << "Do not know object " << objectName << endl;
unlock();
return false;
}
if (!coordSystem.empty() && robot && robot->hasRobotNode(coordSystem))
{
RobotNodePtr rn = robot->getRobotNode(coordSystem);
if (rn)
p = rn->toGlobalCoordinateSystem(p);
}
objects[objectName].object->setGlobalPose(p);
SoNode *n = objects[objectName].visuGrasps->getChild(0);
SoMatrixTransform *mat = dynamic_cast<SoMatrixTransform*>(n);
if (mat)
mat->matrix.setValue(CoinVisualizationFactory::getSbMatrix(p));
n = objects[objectName].visuReachableGrasps->getChild(0);
mat = dynamic_cast<SoMatrixTransform*>(n);
if (mat)
mat->matrix.setValue(CoinVisualizationFactory::getSbMatrix(p));
// delete reachable grasps visu, it may not be valid any more
if (objects[objectName].visuReachableGrasps->getNumChildren()==2)
objects[objectName].visuReachableGrasps->removeChild(1);
if (sceneSep->findChild(objects[objectName].visuReachableGrasps)>=0)
sceneSep->removeChild(objects[objectName].visuReachableGrasps);
unlock();
redraw();
return true;
}
bool SimoxRobotViewer::showCoordSystem( const std::string &jointName, bool enable )
{
if (!robot)
return false;
lock();
if (!robot->hasRobotNode(jointName))
{
VR_ERROR << "Robot does not know RobotNode with name " << jointName << endl;
unlock();
return false;
}
RobotNodePtr rn = robot->getRobotNode(jointName);
if (!rn)
{
unlock();
return false;
}
rn->showCoordinateSystem(enable);
unlock();
setRobot(robot);
return true;
}
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;
}
void SimoxMotionState::setGlobalPoseSimox( const Eigen::Matrix4f& worldPose )
{
if (!sceneObject)
return;
// inv com as matrix4f
Eigen::Matrix4f comLocal;
comLocal.setIdentity();
comLocal.block(0,3,3,1) = -com;
// worldPose -> local visualization frame
/*Eigen::Matrix4f localPose = sceneObject->getGlobalPoseVisualization().inverse() * worldPose;
// com as matrix4f
Eigen::Matrix4f comLocal;
comLocal.setIdentity();
comLocal.block(0,3,3,1) = -com;
//Eigen::Matrix4f localPoseAdjusted = localPose * comLocal;
//Eigen::Matrix4f localPoseAdjusted = comLocal * localPose;
//Eigen::Matrix4f localPoseAdjusted = localPose;
//localPoseAdjusted.block(0,3,3,1) -= com;
Eigen::Matrix4f comTrafo = Eigen::Matrix4f::Identity();
comTrafo.block(0,3,3,1) = -com;
Eigen::Matrix4f localPoseAdjusted = localPose * comTrafo;
//localPoseAdjusted.block(0,3,3,1) -= com;
Eigen::Matrix4f resPose = sceneObject->getGlobalPoseVisualization() * localPoseAdjusted;
*/
Eigen::Matrix4f resPose = worldPose * comLocal;
if (robotNodeActuator)
{
// get joint angle
RobotNodePtr rn = robotNodeActuator->getRobotNode();
DynamicsWorldPtr w = DynamicsWorld::GetWorld();
DynamicsRobotPtr dr = w->getEngine()->getRobot(rn->getRobot());
BulletRobotPtr bdr = boost::dynamic_pointer_cast<BulletRobot>(dr);
if (bdr)
{
// check if robotnode is connected with a joint
std::vector<BulletRobot::LinkInfo> links = bdr->getLinks(rn);
// update all involved joint values
for (size_t i=0;i<links.size();i++)
{
if (links[i].nodeJoint)
{
float ja = bdr->getJointAngle(links[i].nodeJoint);
// we can update the joint value via an RobotNodeActuator
RobotNodeActuatorPtr rna (new RobotNodeActuator(links[i].nodeJoint));
rna->updateJointAngle(ja);
}
}
// we assume that all models are handled by Bullet, so we do not need to update children
robotNodeActuator->updateVisualizationPose(resPose, false);
#if 0
if (rn->getName() == "Shoulder 1 L")
{
cout << "Shoulder 1 L:" << ja << ", speed:" << bdr->getJointSpeed(rn) << endl;
}
#endif
} /*else
{
VR_WARNING << "Could not determine dynamic robot?!" << endl;
}*/
} else
{
sceneObject->setGlobalPose(resPose);
}
}
void reachabilityWindow::createReach()
{
if (!robot || !currentRobotNodeSet)
return;
// setup window
Ui::ReachabilityCreate UICreate;
QDialog diag;
UICreate.setupUi(&diag);
RobotNodePtr baseNode = currentRobotNodeSet->getKinematicRoot();
RobotNodePtr tcpNode = currentRobotNodeSet->getTCP();
UICreate.labelRNS->setText(QString("RobotNodeSet: ") + QString(currentRobotNodeSet->getName().c_str()));
UICreate.labelBaseNode->setText(QString("Base: ") + QString(baseNode->getName().c_str()));
UICreate.labelTCP->setText(QString("TCP: ") + QString(tcpNode->getName().c_str()));
reachSpace.reset(new Reachability(robot));
float minB[6];// = {-1000.0f,-1000.0f,-1000.0f,(float)-M_PI,(float)-M_PI,(float)-M_PI};
float maxB[6];// ={1000.0f,1000.0f,1000.0f,(float)M_PI,(float)M_PI,(float)M_PI};
reachSpace->checkForParameters(currentRobotNodeSet,1000,minB,maxB,baseNode,tcpNode);
//float ex = currentRobotNodeSet->getMaximumExtension();
UICreate.doubleSpinBoxMinX->setValue(minB[0]);
UICreate.doubleSpinBoxMaxX->setValue(maxB[0]);
UICreate.doubleSpinBoxMinY->setValue(minB[1]);
UICreate.doubleSpinBoxMaxY->setValue(maxB[1]);
UICreate.doubleSpinBoxMinZ->setValue(minB[2]);
UICreate.doubleSpinBoxMaxZ->setValue(maxB[2]);
std::vector < VirtualRobot::RobotNodeSetPtr > allRNS;
robot->getRobotNodeSets(allRNS);
for (size_t i=0;i<allRNS.size();i++)
{
UICreate.comboBoxColModelDynamic->addItem(QString(allRNS[i]->getName().c_str()));
UICreate.comboBoxColModelStatic->addItem(QString(allRNS[i]->getName().c_str()));
}
if (diag.exec())
{
minB[0] = UICreate.doubleSpinBoxMinX->value();
minB[1] = UICreate.doubleSpinBoxMinY->value();
minB[2] = UICreate.doubleSpinBoxMinZ->value();
minB[3] = UICreate.doubleSpinBoxMinRo->value();
minB[4] = UICreate.doubleSpinBoxMinPi->value();
minB[5] = UICreate.doubleSpinBoxMinYa->value();
maxB[0] = UICreate.doubleSpinBoxMaxX->value();
maxB[1] = UICreate.doubleSpinBoxMaxY->value();
maxB[2] = UICreate.doubleSpinBoxMaxZ->value();
maxB[3] = UICreate.doubleSpinBoxMaxRo->value();
maxB[4] = UICreate.doubleSpinBoxMaxPi->value();
maxB[5] = UICreate.doubleSpinBoxMaxYa->value();
SceneObjectSetPtr staticModel;
SceneObjectSetPtr dynamicModel;
if (UICreate.checkBoxColDetecion->isChecked())
{
std::string staticM = std::string(UICreate.comboBoxColModelStatic->currentText().toAscii());
std::string dynM = std::string(UICreate.comboBoxColModelDynamic->currentText().toAscii());
staticModel = robot->getRobotNodeSet(staticM);
dynamicModel = robot->getRobotNodeSet(dynM);
}
float discrTr = UICreate.doubleSpinBoxDiscrTrans->value();
float discrRo = UICreate.doubleSpinBoxDiscrRot->value();
reachSpace->initialize(currentRobotNodeSet,discrTr,discrRo,minB,maxB,staticModel,dynamicModel,baseNode,tcpNode);//200.0f,0.4f,minB,maxB,staticModel,dynamicModel,baseNode);
reachSpace->print();
reachSpace->addCurrentTCPPose();
reachSpace->print();
}
}
Eigen::Matrix4f LinkedCoordinate::getInFrame(const RobotNodePtr & destination) const {
if (!destination)
THROW_VR_EXCEPTION(format("RobotNodePtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
if (!this->robot->hasRobotNode( destination) )
THROW_VR_EXCEPTION(format("Robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % destination->getName() % this->robot->getName() % BOOST_CURRENT_FUNCTION);
return LinkedCoordinate::getCoordinateTransformation(this->frame,destination,this->robot) * this->pose;
}
void LinkedCoordinate::changeFrame(const RobotNodePtr & destination) {
if (!destination)
THROW_VR_EXCEPTION(format("RobotNodePtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
if (!this->robot->hasRobotNode( destination) )
THROW_VR_EXCEPTION(format("Robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % destination->getName() % this->robot->getName() % BOOST_CURRENT_FUNCTION);
if (!this->frame)
this->pose = Eigen::Matrix4f::Identity();
else
this->pose = LinkedCoordinate::getCoordinateTransformation(this->frame,destination,this->robot) * this->pose;
this->frame = destination;
}
void LinkedCoordinate::set( const RobotNodePtr &frame,const Eigen::Matrix4f &pose) {
if (!frame)
THROW_VR_EXCEPTION(format("RobotNodePtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
if (!this->robot->hasRobotNode( frame ) )
THROW_VR_EXCEPTION(format("Robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % frame->getName() % this->robot->getName() % BOOST_CURRENT_FUNCTION);
this->pose = pose;
this->frame = frame;
}
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);
}
}
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;
}
