From 5f92d1fe4e0eca373a5f561f0e10e79d8d05d6dd Mon Sep 17 00:00:00 2001
From: vahrenkamp <vahrenkamp@042f3d55-54a8-47e9-b7fb-15903f145c44>
Date: Sun, 25 Nov 2012 19:12:07 +0000
Subject: [PATCH] Improved dynamics robot creation: Mapping from VirtualRobot
Kinematic structure to bullet model. Added ignoreCollision tag to physics
section of robot's xml description: Allows to specify disabled collision
pairs for physics engines. Adjusted ARMAR-III model to work with
bullet-SimDynamics.
git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@340 042f3d55-54a8-47e9-b7fb-15903f145c44
---
.../BulletEngine/BulletEngine.cpp | 4 +-
.../BulletEngine/BulletRobot.cpp | 644 ++++++++++++++++--
.../DynamicsEngine/BulletEngine/BulletRobot.h | 21 +-
.../BulletEngine/SimoxMotionState.cpp | 4 +-
.../BulletDebugViewerGlut.cpp | 8 +-
.../SimDynamicsViewer/SimDynamicsViewer.cpp | 5 +-
.../SimDynamicsViewer/simDynamicsWindow.cpp | 43 +-
VirtualRobot/Nodes/RobotNode.cpp | 40 +-
VirtualRobot/Nodes/RobotNode.h | 63 +-
VirtualRobot/Nodes/RobotNodeFactory.h | 7 +-
VirtualRobot/Nodes/RobotNodeFixed.cpp | 21 +-
VirtualRobot/Nodes/RobotNodeFixed.h | 10 +-
VirtualRobot/Nodes/RobotNodeFixedFactory.cpp | 8 +-
VirtualRobot/Nodes/RobotNodeFixedFactory.h | 4 +-
VirtualRobot/Nodes/RobotNodePrismatic.cpp | 30 +-
VirtualRobot/Nodes/RobotNodePrismatic.h | 11 +-
.../Nodes/RobotNodePrismaticFactory.cpp | 8 +-
.../Nodes/RobotNodePrismaticFactory.h | 4 +-
VirtualRobot/Nodes/RobotNodeRevolute.cpp | 25 +-
VirtualRobot/Nodes/RobotNodeRevolute.h | 10 +-
.../Nodes/RobotNodeRevoluteFactory.cpp | 8 +-
VirtualRobot/Nodes/RobotNodeRevoluteFactory.h | 4 +-
VirtualRobot/SceneObject.cpp | 6 +
VirtualRobot/SceneObject.h | 21 +-
VirtualRobot/Transformation/DHParameter.h | 12 +
VirtualRobot/XML/BaseIO.cpp | 69 +-
VirtualRobot/XML/BaseIO.h | 3 +-
VirtualRobot/XML/RobotIO.cpp | 64 +-
VirtualRobot/XML/RobotIO.h | 9 +-
.../data/robots/ArmarIII/ArmarIII-Head.xml | 5 +
.../data/robots/ArmarIII/ArmarIII-LeftArm.xml | 2 +
.../robots/ArmarIII/ArmarIII-RightArm.xml | 2 +
.../data/robots/ArmarIII/ArmarIII.xml | 11 +-
.../examples/RobotViewer/showRobotWindow.cpp | 37 +
34 files changed, 1009 insertions(+), 214 deletions(-)
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp
index 0e5fbfedf..f8bff2148 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp
@@ -366,7 +366,9 @@ void BulletEngine::print()
std::vector<BulletRobot::LinkInfo> links = br->getLinks();
for (size_t j=0;j<links.size();j++)
{
- cout << "++++ Link " << j << ":" << links[j].node2->getName();
+ cout << "++++ Link " << j << ":" << links[j].nodeJoint->getName();
+ cout << "++++ - ColModelA " << j << ":" << links[j].nodeA->getName();
+ cout << "++++ - ColModelB " << j << ":" << links[j].nodeB->getName();
cout << " enabled:" << links[j].joint->isEnabled() << endl;
boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(links[j].joint);
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
index 5290b12c6..b7609d234 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
@@ -19,6 +19,7 @@
//#define DEBUG_FIXED_OBJECTS
//#define DEBUG_SHOW_LINKS
using namespace VirtualRobot;
+using namespace std;
namespace SimDynamics {
@@ -55,38 +56,474 @@ void BulletRobot::buildBulletModels(bool enableJointMotors)
return;
robotNodes = robot->getRobotNodes();
+ for (size_t i=0;i<robotNodes.size();i++)
+ {
+
+ RobotNodePtr rn = robotNodes[i];
+ CollisionModelPtr colModel = rn->getCollisionModel();
+ if (colModel)
+ {
+ addIgnoredCollisionModels(rn);
+ // search joint and connected model
+ RobotNodePtr bodyA;
+ RobotNodePtr bodyB = rn;
+ RobotNodePtr joint;
+ RobotNodePtr joint2;
+
+ if (rn->isTranslationalJoint() || rn->isRotationalJoint())
+ joint = rn;
+ RobotNodePtr parent = boost::dynamic_pointer_cast<RobotNode>(rn->getParent());
+ while (parent && !bodyA)
+ {
+ if (!parent->getCollisionModel() && (parent->isTranslationalJoint() || parent->isRotationalJoint()))
+ {
+ if (!joint)
+ {
+ joint = parent;
+ } else
+ {
+ // check for hinge2 joint
+ THROW_VR_EXCEPTION_IF(joint2,"three joints in a row not supported:" << joint->getName() << ", " << joint2->getName() << "," << parent->getName());
+ joint2 = parent;
+ Eigen::Matrix4f p1 = joint->getGlobalPoseJoint();
+ Eigen::Matrix4f p2 = joint2->getGlobalPoseJoint();
+
+ float d = (p1.block(0,3,3,1) - p2.block(0,3,3,1)).norm();
+ THROW_VR_EXCEPTION_IF( (d>1e-6), "Could not create hinge2 joint: Joint coord systems must be located at the same position:" << joint->getName() << ", " << joint2->getName());
+ RobotNodeRevolutePtr rev1 = boost::dynamic_pointer_cast<RobotNodeRevolute>(joint);
+ RobotNodeRevolutePtr rev2 = boost::dynamic_pointer_cast<RobotNodeRevolute>(joint2);
+ THROW_VR_EXCEPTION_IF( !rev1 || !rev2 , "Could not create hinge2 joint: Joints must be revolute nodes:" << joint->getName() << ", " << joint2->getName());
+ Eigen::Vector3f ax1 = rev1->getJointRotationAxis();
+ Eigen::Vector3f ax2 = rev2->getJointRotationAxis();
+ float ang = MathTools::getAngle(ax1,ax2);
+ THROW_VR_EXCEPTION_IF( fabs(fabs(ang)-M_PI_2) > 1e-6, "Could not create hinge2 joint: Joint axes must be orthogonal to each other:" << joint->getName() << ", " << joint2->getName());
+ }
+ }
+ if (parent->getCollisionModel())
+ {
+ //if (!joint)
+ // THROW_VR_EXCEPTION("Could not build dynamic model: Robotnode " << parent->getName() << " and " << bodyB->getName() << " got a collision model but they are not linked by a joint.");
+ bodyA = parent;
+ break;
+ }
+
+ parent = boost::dynamic_pointer_cast<RobotNode>(parent->getParent());
+ }
+ if (!bodyA)
+ {
+ // root node
+ /*if (joint)
+ {
+ THROW_VR_EXCEPTION ("Not able to build valid dynamic model: First node of robot with collision model (" << rn->getName() << ") is connected with a joint (" << joint->getName() <<") but no parent collision model found.");
+ }*/
+ bodyA = robot->getRootNode();
+ }
+ //} else
+ {
+ /* Eigen::Matrix4f trafoA2J = Eigen::Matrix4f::Identity(); // bodyA->joint
+ Eigen::Matrix4f trafoJ2B = Eigen::Matrix4f::Identity(); // joint->bodyB
+ if (joint)
+ {
+ // now we have bodyA->joint->bodyB
+ // compute trafoA2J
+ std::vector<Eigen::Matrix4f> trafosA;
+ // visualization is affected by preJointTransformation
+ trafosA.push_back(joint->getPreJointTransformation());
+
+ // go parents up until bodyA is reached
+ parent = boost::dynamic_pointer_cast<RobotNode>(joint->getParent());
+ while (parent && parent!=bodyA)
+ {
+ trafosA.push_back(parent->getPostJointTransformation());
+ trafosA.push_back(parent->getPreJointTransformation());
+ parent = boost::dynamic_pointer_cast<RobotNode>(parent->getParent());
+ }
+ THROW_VR_EXCEPTION_IF(!parent,"internal error, no parent 2");
+ trafosA.push_back(bodyA->getPostJointTransformation());
+ std::vector<Eigen::Matrix4f>::reverse_iterator rit;
+ // accumulate transformations from bodyA_post to joint_pre
+ for (rit=trafosA.rbegin(); rit != trafosA.rend(); rit++)
+ {
+ trafoA2J *= *rit;
+ }
+
+ // compute trafoJ2B
+ std::vector<Eigen::Matrix4f> trafosB;
+
+ // visualization is affected by preJointTransformation
+ trafosB.push_back(bodyB->getPreJointTransformation());
+ if (joint!=bodyB)
+ {
+ // go parents up until joint is reached
+ parent = boost::dynamic_pointer_cast<RobotNode>(bodyB->getParent());
+ while (parent && parent!=joint)
+ {
+ trafosB.push_back(parent->getPostJointTransformation());
+ trafosB.push_back(parent->getPreJointTransformation());
+ parent = boost::dynamic_pointer_cast<RobotNode>(parent->getParent());
+ }
+ THROW_VR_EXCEPTION_IF(!parent,"internal error, no parent");
+ trafosB.push_back(joint->getPostJointTransformation());
+ }
+ // accumulate transformations from joint_post to bodyB_pre
+ for (rit=trafosB.rbegin(); rit != trafosB.rend(); rit++)
+ {
+ trafoJ2B *= *rit;
+ }
+ } else
+ {
+ // fixed joint: bodyA -> bodyB
+ joint = bodyB;
+
+ // compute trafoA2J (which is trafo from A to B)
+ std::vector<Eigen::Matrix4f> trafosA;
+ // visualization is affected by preJointTransformation
+ trafosA.push_back(bodyB->getPreJointTransformation());
+
+ // go parents up until bodyA is reached
+ parent = boost::dynamic_pointer_cast<RobotNode>(bodyB->getParent());
+ while (parent && parent!=bodyA)
+ {
+ trafosA.push_back(parent->getPostJointTransformation());
+ trafosA.push_back(parent->getPreJointTransformation());
+ parent = boost::dynamic_pointer_cast<RobotNode>(parent->getParent());
+ }
+ THROW_VR_EXCEPTION_IF(!parent,"internal error, no parent 3");
+ trafosA.push_back(bodyA->getPostJointTransformation());
+ std::vector<Eigen::Matrix4f>::reverse_iterator rit;
+ // accumulate transformations from bodyA_post to joint_pre
+ for (rit=trafosA.rbegin(); rit != trafosA.rend(); rit++)
+ {
+ trafoA2J *= *rit;
+ }
+
+ }*/
+
+ // check for fixed joint
+ if (!joint)
+ joint = bodyB;
+
+ createLink(bodyA,joint,joint2,bodyB);//,trafoA2J,trafoJ2B);
+ }
+
+ }
+
+ /*
+ RobotNodePtr bodyA;
+ RobotNodePtr bodyB;
+ RobotNodePtr joint;
+ Eigen::Matrix4f trafoA = Eigen::Matrix4f;
+ Eigen::Matrix4f trafoB = Eigen::Matrix4f;
for (size_t i=0;i<robotNodes.size();i++)
{
+
RobotNodePtr rn = robotNodes[i];
+
+ switch(rn->getType())
+ {
+ case RobotNode::Generic:
+ THROW_VR_EXCEPTION ("Could not build dynamic model. Only <JointNode> <TransformationNode> and <BodyNode> tags allowed!");
+ break;
+ case RobotNode::Body:
+ break;
+ default:
+ continue;
+ }
+ // a body
+ bodyB = rn;
+ bodyA.reset();
+ joint.reset();
+ trafoA = Eigen::Matrix4f;
+ trafoB = Eigen::Matrix4f;
+
+ // search parent body
+ RobotNodePtr parent = rn->getParent();
+ bool built = false;
+ while (parent && !built)
+ {
+ switch (parent->getType())
+ {
+ case RobotNode::Joint:
+ if (joint)
+ {
+ THROW_VR_EXCEPTION("Need bodies between joints. Two succeeding joints found in kinematic structure");
+ }
+ joint = parent;
+ break;
+ case RobotNode::Body:
+ if (!joint)
+ {
+ THROW_VR_EXCEPTION("Need joint between body. Two succeeding bodies found in kinematic structure");
+ }
+ createLink(bodyA,joint,bodyB,trafoA,trafoB);
+ built = true;
+ break;
+ case RobotNode::Transform:
+ if (!joint)
+ trafoA *= parent->getPostJointTransformation
+ built = true;
+ break;
+ }
+ }
+
+ */
+
+ /*
std::vector<SceneObjectPtr> children = rn->getChildren();
for (size_t c=0;c<children.size();c++)
{
- // check what to do
- /*
- bool fixed = !(children[c]->isRotationalJoint() || children[c]->isTranslationalJoint());
- if (fixed && !(children[c]->getCollisionModel()))
- continue; // a fixed transformation without model, skip it
- */
RobotNodePtr rn2 = boost::dynamic_pointer_cast<RobotNode>(children[c]);
if (rn2)
createLink(rn,rn2,enableJointMotors);
- }
+ }*/
}
}
-void BulletRobot::createLink( VirtualRobot::RobotNodePtr node1,VirtualRobot::RobotNodePtr node2, bool enableJointMotors )
+void BulletRobot::addIgnoredCollisionModels(RobotNodePtr rn)
{
- THROW_VR_EXCEPTION_IF (!(node1->hasChild(node2)),"node1 must be parent of node2");
+ VR_ASSERT (rn);
+ if (!rn->getCollisionModel())
+ return; // nothing to do: no col model -> no bullet model -> no collisions
+ createDynamicsNode(rn);
+ std::vector<std::string> ic = rn->getIgnoredCollisionModels();
+ RobotPtr robot = rn->getRobot();
+ BulletObjectPtr drn1 = boost::dynamic_pointer_cast<BulletObject>(dynamicRobotNodes[rn]);
+ VR_ASSERT(drn1);
+
+ for (size_t i=0;i<ic.size();i++)
+ {
+ RobotNodePtr rn2 = robot->getRobotNode(ic[i]);
+ if (!rn2)
+ {
+ VR_ERROR << "Error while processing robot node <" << rn->getName() << ">: Ignored collision model <" << ic[i] << "> is not part of robot..." << endl;
+ } else
+ {
+ createDynamicsNode(rn2);
+ BulletObjectPtr drn2 = boost::dynamic_pointer_cast<BulletObject>(dynamicRobotNodes[rn2]);
+ VR_ASSERT(drn2);
+ DynamicsWorld::GetWorld()->getEngine()->disableCollision(drn1.get(),drn2.get());
+ }
+ }
+}
- if (dynamicRobotNodes.find(node1) == dynamicRobotNodes.end())
- {
- createDynamicsNode(node1);
- }
- if (dynamicRobotNodes.find(node2) == dynamicRobotNodes.end())
+void BulletRobot::createLink( VirtualRobot::RobotNodePtr bodyA, VirtualRobot::RobotNodePtr joint, VirtualRobot::RobotNodePtr joint2, VirtualRobot::RobotNodePtr bodyB, bool enableJointMotors )
+{
+ // ensure dynamics nodes are created
+ createDynamicsNode(bodyA);
+ createDynamicsNode(bodyB);
+
+ if (hasLink(bodyA,bodyB))
+ {
+ THROW_VR_EXCEPTION("Joints are already connected:" << bodyA->getName() << "," << bodyB->getName());
+ }
+ BulletObjectPtr drn1 = boost::dynamic_pointer_cast<BulletObject>(dynamicRobotNodes[bodyA]);
+ BulletObjectPtr drn2 = boost::dynamic_pointer_cast<BulletObject>(dynamicRobotNodes[bodyB]);
+ VR_ASSERT(drn1);
+ VR_ASSERT(drn2);
+ boost::shared_ptr<btRigidBody> btBody1 = drn1->getRigidBody();
+ boost::shared_ptr<btRigidBody> btBody2 = drn2->getRigidBody();
+ VR_ASSERT(btBody1);
+ VR_ASSERT(btBody2);
+
+
+ Eigen::Matrix4f coordSystemNode1 = bodyA->getGlobalPoseJoint(); // todo: what if joint is not at 0 ?!
+ Eigen::Matrix4f coordSystemNode2 = bodyB->getGlobalPoseJoint();
+ Eigen::Matrix4f coordSystemJoint = joint->getGlobalPoseJoint();
+
+ Eigen::Matrix4f anchorPointGlobal = joint->getGlobalPoseJoint();//node1->getGlobalPose() * node2->getPreJointTransformation(); //
+
+ Eigen::Matrix4f anchor_inNode1 = coordSystemNode1.inverse() * anchorPointGlobal;
+ Eigen::Matrix4f anchor_inNode2 = coordSystemNode2.inverse() * anchorPointGlobal;
+
+
+ // The bullet model was adjusted, so that origin is at local com
+ // since we computed the anchor in from simox models, we must re-adjust the anchor, in order to consider the com displacement
+ Eigen::Matrix4f com1;
+ com1.setIdentity();
+ com1.block(0,3,3,1) = -drn1->getCom();
+ anchor_inNode1 = com1 * anchor_inNode1;
+
+ Eigen::Matrix4f com2;
+ com2.setIdentity();
+ com2.block(0,3,3,1) = -drn2->getCom();
+ anchor_inNode2 = com2 * anchor_inNode2;
+
+ boost::shared_ptr<btTypedConstraint> jointbt;
+
+ float vr2bulletOffset = 0.0f;
+
+ THROW_VR_EXCEPTION_IF (joint->isTranslationalJoint(), "Translational joints nyi...");
+ if (joint->isRotationalJoint())
+ {
+ // create joint
+ boost::shared_ptr<RobotNodeRevolute> rnRevJoint = boost::dynamic_pointer_cast<RobotNodeRevolute>(joint);
+
+ // transform axis direction (not position!)
+ Eigen::Vector4f axisLocalJoint = Eigen::Vector4f::Zero();
+ axisLocalJoint.block(0,0,3,1) = rnRevJoint->getJointRotationAxisInJointCoordSystem();
+ Eigen::Matrix4f tmpGpJoint = coordSystemJoint;
+ tmpGpJoint.block(0,3,3,1).setZero(); // coordSystemJoint
+ Eigen::Vector4f axisGlobal = tmpGpJoint * axisLocalJoint;
+
+ float limMin,limMax;
+ limMin = joint->getJointLimitLo();
+ limMax = joint->getJointLimitHi();
+
+ if (joint2)
+ {
+
+ VR_WARNING << "HINGE2 Joints are experimental (1:" << joint->getName() << ", 2:" << joint2->getName() << "): Assuming hing2/universal joint is defined as needed by bullet (see universal constraint header documentation)" << endl;
+ // UNIVERSAL/HINGE2 joint
+ boost::shared_ptr<RobotNodeRevolute> rnRevJoint2 = boost::dynamic_pointer_cast<RobotNodeRevolute>(joint2);
+ THROW_VR_EXCEPTION_IF (!rnRevJoint2, "Second joint must be a revolute joint...");
+ Eigen::Matrix4f coordSystemJoint2 = joint2->getGlobalPoseJoint();
+
+ Eigen::Vector4f axisLocalJoint2= Eigen::Vector4f::Zero();
+ axisLocalJoint2.block(0,0,3,1) = rnRevJoint2->getJointRotationAxisInJointCoordSystem();
+ Eigen::Matrix4f tmpGpJoint2 = coordSystemJoint2;
+ tmpGpJoint2.block(0,3,3,1).setZero();
+
+ Eigen::Vector4f axisGlobal2 = tmpGpJoint2 * axisLocalJoint2;
+
+
+ btVector3 axis1 = BulletEngine::getVecBullet(axisGlobal.head(3),false);
+ btVector3 axis2 = BulletEngine::getVecBullet(axisGlobal2.head(3),false);
+ btVector3 pivot = BulletEngine::getVecBullet(anchorPointGlobal.block(0,3,3,1));
+ boost::shared_ptr<btUniversalConstraint> hinge2(new btUniversalConstraint(*btBody1, *btBody2, pivot, axis1, axis2));
+ float limMin2,limMax2;
+ limMin2 = joint2->getJointLimitLo();
+ limMax2 = joint2->getJointLimitHi();
+ hinge2->setLowerLimit(btScalar(limMin),btScalar(limMin2));
+ hinge2->setUpperLimit(btScalar(limMax),btScalar(limMax2));
+ jointbt = hinge2;
+ } else
+ {
+ // HINGE joint
+ Eigen::Matrix4f tmpGp1 = coordSystemNode1;
+ tmpGp1.block(0,3,3,1).setZero();
+ Eigen::Matrix4f tmpGp2 = coordSystemNode2;
+ tmpGp2.block(0,3,3,1).setZero();
+ Eigen::Vector3f axis_inLocal1 = (tmpGp1.inverse() * axisGlobal).block(0,0,3,1);
+ Eigen::Vector3f axis_inLocal2 = (tmpGp2.inverse() * axisGlobal).block(0,0,3,1);
+ //Eigen::Vector3f axis_inLocal2 = rnRev2->getJointRotationAxisInJointCoordSystem();
+
+ btVector3 pivot1 = BulletEngine::getVecBullet(anchor_inNode1.block(0,3,3,1));
+ btVector3 pivot2 = BulletEngine::getVecBullet(anchor_inNode2.block(0,3,3,1));
+
+
+
+ #ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
+ THROW_VR_EXCEPTION ("USE_BULLET_GENERIC_6DOF_CONSTRAINT nyi in this method...");
+ #endif
+ //btVector3 axis1 = BulletEngine::getVecBullet(axis_inLocal1,false);
+ //btVector3 axis2 = BulletEngine::getVecBullet(axis_inLocal2,false);
+ //boost::shared_ptr<btHingeConstraint> hinge(new btHingeConstraint(*btBody1, *btBody2, pivot1, pivot2, axis1, axis2,false));
+
+ // we need to align coord system joint, so that z-axis is rotation axis
+ Eigen::Vector3f axisGlobal = rnRevJoint->getJointRotationAxis();
+ Eigen::Vector3f axisLocal = rnRevJoint->getJointRotationAxisInJointCoordSystem();
+ MathTools::Quaternion q1 = MathTools::getRotation(Eigen::Vector3f::UnitZ(),axisLocal);
+ Eigen::Matrix4f rotationzAlignment = MathTools::quat2eigen4f(q1);
+ Eigen::Matrix4f coordSystemJoint_zAligned = coordSystemJoint * rotationzAlignment ;
+
+ // get transformation coord1 -> joint coord
+ Eigen::Matrix4f trafoNode1 = coordSystemNode1.inverse() * coordSystemJoint_zAligned;
+ // get transformation coord2 -> joint coord
+ Eigen::Matrix4f trafoNode2 = coordSystemNode2.inverse() * coordSystemJoint_zAligned;
+
+ // now we need to pivot points in local coord systems
+ btTransform tr1 = BulletEngine::getPoseBullet(trafoNode1);
+ btTransform tr2 = BulletEngine::getPoseBullet(trafoNode2);
+ tr1.getOrigin() = pivot1;
+ tr2.getOrigin() = pivot2;
+
+ boost::shared_ptr<btHingeConstraint> hinge(new btHingeConstraint(*btBody1, *btBody2, tr1, tr2, true));
+
+ // todo: check effects of parameters...
+ //hinge->setParam(BT_CONSTRAINT_STOP_ERP,0.9f);
+ /*
+ hinge->setParam(BT_CONSTRAINT_CFM,0.9f);
+ hinge->setParam(BT_CONSTRAINT_STOP_CFM,0.01f);*/
+ //hinge->setLimit(limMin,limMax);//,btScalar(1.0f));
+ //hinge->setParam(BT_CONSTRAINT_CFM,1.0f);
+
+ btScalar startAngle = joint->getJointValue();
+ btScalar startAngleBT = hinge->getHingeAngle();
+ btScalar limMinBT, limMaxBT;
+ btScalar diff = joint->getJointValueOffset();//startAngleBT + startAngle);
+ limMinBT = limMin + diff;//diff - limMax;//
+ limMaxBT = limMax + diff;//diff - limMin;//
+ if (fabs(startAngleBT - startAngle)>1e-6)
+ {
+ cout << "joint " << joint->getName() << ": jv diff:" << diff << endl;
+ cout << "Simox limits: " << limMin << "/" << limMax << ", bullet limits:" << limMinBT << "/" << limMaxBT << endl;
+ }
+ hinge->setLimit(btScalar(limMinBT),btScalar(limMaxBT));
+ vr2bulletOffset = diff;
+ //hinge->setLimit(btScalar(limMin),btScalar(limMax));
+ //hinge->setAngularOnly(true);
+ jointbt = hinge;
+ }
+ } else
+ {
+ // create fixed joint
+ btTransform localA,localB;
+ localA = BulletEngine::getPoseBullet(anchor_inNode1);
+ localB = BulletEngine::getPoseBullet(anchor_inNode2);
+ boost::shared_ptr<btGeneric6DofConstraint> generic6Dof(new btGeneric6DofConstraint(*btBody1, *btBody2, localA, localB, true));
+ generic6Dof->setOverrideNumSolverIterations(100);
+
+ for (int i=0;i<6;i++)
+ generic6Dof->setLimit(i,0,0);
+
+ /*generic6Dof->setParam(BT_CONSTRAINT_STOP_CFM,0.01f,0);
+ generic6Dof->setParam(BT_CONSTRAINT_STOP_CFM,0.01f,1);
+ generic6Dof->setParam(BT_CONSTRAINT_STOP_CFM,0.01f,2);
+ generic6Dof->setParam(BT_CONSTRAINT_STOP_CFM,0.01f,3);
+ generic6Dof->setParam(BT_CONSTRAINT_STOP_CFM,0.01f,4);
+ generic6Dof->setParam(BT_CONSTRAINT_STOP_CFM,0.01f,5);*/
+ /*generic6Dof->setParam(BT_CONSTRAINT_STOP_ERP,0.9f,0);
+ generic6Dof->setParam(BT_CONSTRAINT_STOP_ERP,0.9f,1);
+ generic6Dof->setParam(BT_CONSTRAINT_STOP_ERP,0.9f,2);
+ generic6Dof->setParam(BT_CONSTRAINT_STOP_ERP,0.9f,3);
+ generic6Dof->setParam(BT_CONSTRAINT_STOP_ERP,0.9f,4);
+ generic6Dof->setParam(BT_CONSTRAINT_STOP_ERP,0.9f,5);*/
+ jointbt = generic6Dof;
+ }
+ LinkInfo i;
+ i.nodeA = bodyA;
+ i.nodeB = bodyB;
+ i.dynNode1 = drn1;
+ i.dynNode2 = drn2;
+ i.nodeJoint = joint;
+ i.nodeJoint2 = joint2;
+ i.joint = jointbt;
+ i.jointValueOffset = vr2bulletOffset;
+
+ // disable col model
+ i.disabledCollisionPairs.push_back(
+ std::pair<DynamicsObjectPtr,DynamicsObjectPtr>(
+ boost::dynamic_pointer_cast<DynamicsObject>(drn1),
+ boost::dynamic_pointer_cast<DynamicsObject>(drn2)));
+
+ links.push_back(i);
+#ifndef DEBUG_FIXED_OBJECTS
+ if (enableJointMotors && joint->isRotationalJoint())
{
- createDynamicsNode(node2);
+ // start standard actuator
+ actuateNode(joint,joint->getJointValue());
}
+#endif
+}
+
+void BulletRobot::createLink( VirtualRobot::RobotNodePtr node1,VirtualRobot::RobotNodePtr node2, bool enableJointMotors )
+{
+ THROW_VR_EXCEPTION_IF (!(node1->hasChild(node2)),"node1 must be parent of node2");
+
+ // ensure dynamics nodes are created
+ createDynamicsNode(node1);
+ createDynamicsNode(node2);
if (hasLink(node1,node2))
{
@@ -102,8 +539,6 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr node1,VirtualRobot::Rob
VR_ASSERT(btBody1);
VR_ASSERT(btBody2);
-
-
Eigen::Matrix4f coordSystemNode1 = node1->getGlobalPoseJoint(); // todo: what if joint is not at 0 ?!
Eigen::Matrix4f coordSystemNode2 = node2->getGlobalPoseJoint();
@@ -385,8 +820,9 @@ void BulletRobot::createLink( VirtualRobot::RobotNodePtr node1,VirtualRobot::Rob
#endif
}
LinkInfo i;
- i.node1 = node1;
- i.node2 = node2;
+ i.nodeA = node1;
+ i.nodeB = node2;
+ i.nodeJoint = node2;
i.dynNode1 = drn1;
i.dynNode2 = drn2;
i.joint = joint;
@@ -433,7 +869,7 @@ bool BulletRobot::hasLink( VirtualRobot::RobotNodePtr node1, VirtualRobot::Robot
{
for (size_t i=0; i<links.size();i++)
{
- if (links[i].node1 == node1 && links[i].node2 == node2)
+ if (links[i].nodeA == node1 && links[i].nodeB == node2)
return true;
}
return false;
@@ -490,29 +926,57 @@ void BulletRobot::actuateJoints(float dt)
m->m_enableMotor = false;
#else
boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
- VR_ASSERT(hinge);
- if (it->second.enabled)
- {
- btScalar targ = btScalar(it->second.jointValueTarget+link.jointValueOffset);
- //btScalar act = btScalar(it->first->getJointValue());
- btScalar act = btScalar(getJointAngle(it->first));
- hinge->enableAngularMotor(true,(targ-act),10.0f);
- //hinge->enableMotor(true);
- //hinge->setMotorTarget(it->second.jointValueTarget+link.jointValueOffset,dt);
- } else
- hinge->enableMotor(false);
+ if (!hinge)
+ {
+ // hinge2 / universal joint
+ boost::shared_ptr<btUniversalConstraint> hinge2 = boost::dynamic_pointer_cast<btUniversalConstraint>(link.joint);
+ VR_ASSERT(hinge2);
+ btRotationalLimitMotor *m;
+ if (it->second.node==link.nodeJoint)
+ {
+ m = hinge2->getRotationalLimitMotor(1); // second motor
+ } else
+ {
+ VR_ASSERT(it->second.node==link.nodeJoint2);
+ m = hinge2->getRotationalLimitMotor(2); // third motor
+ }
+ VR_ASSERT(m);
+ if (it->second.enabled)
+ {
+ btScalar targ = btScalar(it->second.jointValueTarget+link.jointValueOffset);
+ //btScalar act = btScalar(it->first->getJointValue());
+ btScalar act = btScalar(getJointAngle(it->first));
+ m->m_enableMotor = true;
+ m->m_targetVelocity = (targ-act); // inverted joint dir?!
+ } else
+ m->m_enableMotor = false;
+ } else
+ {
+ if (it->second.enabled)
+ {
+ btScalar targ = btScalar(it->second.jointValueTarget+link.jointValueOffset);
+ //btScalar act = btScalar(it->first->getJointValue());
+ btScalar act = btScalar(getJointAngle(it->first));
+ hinge->enableAngularMotor(true,(targ-act),10.0f);
+ //hinge->enableMotor(true);
+ //hinge->setMotorTarget(it->second.jointValueTarget+link.jointValueOffset,dt);
+ } else
+ hinge->enableMotor(false);
+ }
#endif
}
it++;
}
+
+ setPoseNonActuatedRobotNodes();
}
BulletRobot::LinkInfo BulletRobot::getLink( VirtualRobot::RobotNodePtr node )
{
for (size_t i=0;i<links.size();i++)
{
- if (links[i].node2 == node)
+ if (links[i].nodeJoint == node || links[i].nodeJoint2 == node)
return links[i];
}
THROW_VR_EXCEPTION("No link with node " << node->getName());
@@ -523,7 +987,7 @@ bool BulletRobot::hasLink( VirtualRobot::RobotNodePtr node )
{
for (size_t i=0;i<links.size();i++)
{
- if (links[i].node2 == node)
+ if (links[i].nodeJoint == node || links[i].nodeJoint2 == node)
return true;
}
return false;
@@ -532,8 +996,14 @@ bool BulletRobot::hasLink( VirtualRobot::RobotNodePtr node )
void BulletRobot::actuateNode( VirtualRobot::RobotNodePtr node, float jointValue )
{
VR_ASSERT(node);
+
if (node->isRotationalJoint())
{
+ if (!hasLink(node))
+ {
+ VR_ERROR << "No link for node " << node->getName() << endl;
+ return;
+ }
LinkInfo link = getLink(node);
#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
boost::shared_ptr<btGeneric6DofConstraint> dof = boost::dynamic_pointer_cast<btGeneric6DofConstraint>(link.joint);
@@ -546,8 +1016,28 @@ void BulletRobot::actuateNode( VirtualRobot::RobotNodePtr node, float jointValue
DynamicsRobot::actuateNode(node,jointValue);
#else
boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
- VR_ASSERT(hinge);
- hinge->enableAngularMotor(true,0.0f,bulletMaxMotorImulse);// is max impulse ok?! (10 seems to be ok, 1 oscillates)
+ if (!hinge)
+ {
+ // hinge2 / universal joint
+ boost::shared_ptr<btUniversalConstraint> hinge2 = boost::dynamic_pointer_cast<btUniversalConstraint>(link.joint);
+ VR_ASSERT(hinge2);
+ btRotationalLimitMotor *m;
+ if (node==link.nodeJoint)
+ {
+ m = hinge2->getRotationalLimitMotor(1); // second motor
+ } else
+ {
+ VR_ASSERT(node==link.nodeJoint2);
+ m = hinge2->getRotationalLimitMotor(2); // third motor
+ }
+ VR_ASSERT(m);
+ m->m_enableMotor = true;
+ m->m_maxMotorForce = 5;//bulletMaxMotorImulse; //?!
+ m->m_maxLimitForce = 300;
+ } else
+ {
+ hinge->enableAngularMotor(true,0.0f,bulletMaxMotorImulse);// is max impulse ok?! (10 seems to be ok, 1 oscillates)
+ }
DynamicsRobot::actuateNode(node,jointValue); // inverted joint direction in bullet
#endif
} else
@@ -582,8 +1072,23 @@ float BulletRobot::getJointAngle( VirtualRobot::RobotNodePtr rn )
boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
if (!hinge)
{
- //VR_WARNING << "RobotNode " << rn->getName() << " is not associated with a hinge joint?!" << endl;
- return 0.0f;
+ // hinge2 / universal joint
+ boost::shared_ptr<btUniversalConstraint> hinge2 = boost::dynamic_pointer_cast<btUniversalConstraint>(link.joint);
+ if (!hinge2)
+ return 0.0f;
+ btRotationalLimitMotor *m;
+ if (rn==link.nodeJoint)
+ {
+ m = hinge2->getRotationalLimitMotor(1); // second motor
+ } else if (rn==link.nodeJoint2)
+ {
+ m = hinge2->getRotationalLimitMotor(2); // third motor
+ } else
+ return 0.0f;
+ VR_ASSERT(m);
+ hinge2->calculateTransforms();
+ float a2 = m->m_currentPosition;
+ return (a2-link.jointValueOffset);// inverted joint direction in bullet
}
return (hinge->getHingeAngle()-link.jointValueOffset);// inverted joint direction in bullet
#endif
@@ -607,9 +1112,22 @@ float BulletRobot::getJointSpeed( VirtualRobot::RobotNodePtr rn )
#else
boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
if (!hinge)
- {
- VR_WARNING << "RobotNode " << rn->getName() << " is not associated with a hinge joint?!" << endl;
- return 0.0f;
+ {
+ // hinge2 / universal joint
+ boost::shared_ptr<btUniversalConstraint> hinge2 = boost::dynamic_pointer_cast<btUniversalConstraint>(link.joint);
+ if (!hinge2)
+ return 0.0f;
+ btRotationalLimitMotor *m;
+ if (rn==link.nodeJoint)
+ {
+ m = hinge2->getRotationalLimitMotor(1); // second motor
+ } else if (rn==link.nodeJoint2)
+ {
+ m = hinge2->getRotationalLimitMotor(2); // third motor
+ } else
+ return 0.0f;
+ VR_ASSERT(m);
+ return m->m_targetVelocity;
}
return hinge->getMotorTargetVelosity();
#endif
@@ -636,4 +1154,50 @@ Eigen::Matrix4f BulletRobot::getComGlobal( VirtualRobot::RobotNodePtr rn )
return bo->getComGlobal();
}
+void BulletRobot::setPoseNonActuatedRobotNodes()
+{
+ VR_ASSERT(robot);
+ std::vector<RobotNodePtr> rns = robot->getRobotNodes();
+ std::vector<RobotNodePtr> actuatedNodes;
+ std::vector<RobotNodePtr> notActuatedNodes;
+ size_t i;
+ // only objects with collisionmodel are processed by bullet
+ for (i=0;i<rns.size();i++)
+ {
+ if (rns[i]->getCollisionModel())
+ actuatedNodes.push_back(rns[i]);
+ else
+ notActuatedNodes.push_back(rns[i]);
+ }
+ size_t lastSize = notActuatedNodes.size();
+ while (notActuatedNodes.size()>0)
+ {
+ vector<RobotNodePtr>::iterator it = notActuatedNodes.begin();
+ while (it!=notActuatedNodes.end())
+ {
+ SceneObjectPtr parent = (*it)->getParent();
+ if (!parent || find(actuatedNodes.begin(), actuatedNodes.end(), parent)!=actuatedNodes.end())
+ {
+ // parent is at correct pose, we can update *it
+ if (parent)
+ (*it)->updatePose(false);
+
+ // if root, we also have to delete node from list
+ actuatedNodes.push_back(*it);
+ it = notActuatedNodes.erase(it);
+ } else
+ it++;
+ }
+
+
+ // just a sanity check
+ if (lastSize == notActuatedNodes.size())
+ {
+ VR_ERROR << "Internal error?!" << endl;
+ return;
+ } else
+ lastSize = notActuatedNodes.size();
+ }
+}
+
} // namespace VirtualRobot
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
index 5f68a801e..430eff56b 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
@@ -47,8 +47,10 @@ public:
struct LinkInfo
{
- VirtualRobot::RobotNodePtr node1; // parent
- VirtualRobot::RobotNodePtr node2; // child
+ VirtualRobot::RobotNodePtr nodeA; // parent
+ VirtualRobot::RobotNodePtr nodeB; // child
+ VirtualRobot::RobotNodePtr nodeJoint; // joint
+ VirtualRobot::RobotNodePtr nodeJoint2; // joint2 (only used for hinge2/universal joints)
BulletObjectPtr dynNode1; // parent
BulletObjectPtr dynNode2; // child
std::vector< std::pair<DynamicsObjectPtr,DynamicsObjectPtr> > disabledCollisionPairs;
@@ -100,9 +102,22 @@ protected:
void buildBulletModels(bool enableJointMotors);
+ //void createLink( VirtualRobot::RobotNodePtr bodyA, VirtualRobot::RobotNodePtr joint, VirtualRobot::RobotNodePtr bodyB, Eigen::Matrix4f &trafoA2J, Eigen::Matrix4f &trafoJ2B, bool enableJointMotors = true );
+ // Possible joint types:
+ // fixed (joint=fixed !joint2)
+ // hinge (joint=revolute !joint2)
+ // universal (hinge2) (joint=revolute joint2=revolute) // experimental
+ void createLink( VirtualRobot::RobotNodePtr bodyA, VirtualRobot::RobotNodePtr joint, VirtualRobot::RobotNodePtr joint2, VirtualRobot::RobotNodePtr bodyB, bool enableJointMotors = true );
+
void createLink(VirtualRobot::RobotNodePtr node1,VirtualRobot::RobotNodePtr node2, bool enableJointMotors);
- std::vector<LinkInfo> links;
+ // ensure that all robot nodes, which are not actuated directly, are at the correct pose
+ void setPoseNonActuatedRobotNodes();
+
+ // process all ignoreCollision tags of physics section of RobotNode. Adds according collision disabled information to physics engine.
+ void addIgnoredCollisionModels(VirtualRobot::RobotNodePtr rn);
+
+ std::vector<LinkInfo> links;
btScalar bulletMaxMotorImulse;
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp b/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp
index cb3b52731..8659a06c6 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp
@@ -121,10 +121,10 @@ void SimoxMotionState::setGlobalPoseSimox( const Eigen::Matrix4f& worldPose )
// we assume that all models are handled by Bullet, so we do not need to update children
robotNodeActuator->updateVisualizationPose(resPose, ja, false);
- } else
+ } /*else
{
VR_WARNING << "Could not determine dynamic robot?!" << endl;
- }
+ }*/
} else
{
sceneObject->setGlobalPose(resPose);
diff --git a/SimDynamics/examples/BulletDebugViewerGlut/BulletDebugViewerGlut.cpp b/SimDynamics/examples/BulletDebugViewerGlut/BulletDebugViewerGlut.cpp
index d9c5e4f82..ddc11dbc7 100644
--- a/SimDynamics/examples/BulletDebugViewerGlut/BulletDebugViewerGlut.cpp
+++ b/SimDynamics/examples/BulletDebugViewerGlut/BulletDebugViewerGlut.cpp
@@ -26,14 +26,14 @@ int main(int argc,char* argv[])
world->addObject(dynObj);
- //std::string robFile("robots/examples/SimpleRobot/Joint6.xml");
- std::string robFile("robots/iCub/iCub.xml");
+ //std::string robFile("robots/examples/SimpleRobot/Joint5.xml");
+ //std::string robFile("robots/iCub/iCub.xml");
//std::string robFile("robots/iCub/iCub_LeftLegTest.xml");
- //std::string robFile("robots/ArmarIII/ArmarIII-RightArm.xml");
+ //std::string robFile("robots/ArmarIII/ArmarIII-RightArmTest5.xml");
//std::string robFile("robots/ArmarIII/ArmarIII-RightHandTest.xml");
//std::string robFile("robots/ArmarIII/ArmarIII-HeadTest.xml");
//std::string robFile("robots/ArmarIII/ArmarIII-RightArmTest2.xml");
- //std::string robFile("robots/ArmarIII/ArmarIII.xml");
+ std::string robFile("robots/ArmarIII/ArmarIII.xml");
//std::string robFile("robots/iCub/iCub_RightHand.xml");
//std::string robFile("robots/iCub/iCub_testFinger.xml");
VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(robFile);
diff --git a/SimDynamics/examples/SimDynamicsViewer/SimDynamicsViewer.cpp b/SimDynamics/examples/SimDynamicsViewer/SimDynamicsViewer.cpp
index 98bb05558..e24b2f3f0 100644
--- a/SimDynamics/examples/SimDynamicsViewer/SimDynamicsViewer.cpp
+++ b/SimDynamics/examples/SimDynamicsViewer/SimDynamicsViewer.cpp
@@ -37,8 +37,9 @@ int main(int argc, char *argv[])
cout << " --- START --- " << endl;
// --robot "robots/iCub/iCub.xml"
//std::string filename("robots/iCub/iCub.xml");
- //std::string filename("robots/ArmarIII/ArmarIII.xml");
- std::string filename("robots/examples/SimpleRobot/Joint6.xml");
+ std::string filename("robots/ArmarIII/ArmarIII.xml");
+ //std::string filename("robots/examples/SimpleRobot/Joint6.xml");
+ //std::string filename("robots/ArmarIII/ArmarIII-RightArmTest6.xml");
if (VirtualRobot::RuntimeEnvironment::hasValue("robot"))
{
diff --git a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
index 831c4c2ec..d7cfafb0c 100644
--- a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
+++ b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
@@ -276,13 +276,20 @@ bool SimDynamicsWindow::loadRobot(std::string robotFilename)
cout << " ERROR while creating robot" << endl;
return false;
}
- //robot->print();
- Eigen::Matrix4f gp = Eigen::Matrix4f::Identity();
- gp(2,3) = 200.0f;
- robot->setGlobalPose(gp);
- dynamicsRobot = dynamicsWorld->CreateDynamicsRobot(robot);
- dynamicsWorld->addRobot(dynamicsRobot);
-
+ try
+ {
+ //robot->print();
+ Eigen::Matrix4f gp = Eigen::Matrix4f::Identity();
+ gp(2,3) = 200.0f;
+ robot->setGlobalPose(gp);
+ dynamicsRobot = dynamicsWorld->CreateDynamicsRobot(robot);
+ dynamicsWorld->addRobot(dynamicsRobot);
+ }catch (VirtualRobotException &e)
+ {
+ cout << " ERROR while building dynamic robot" << endl;
+ cout << e.what();
+ return false;
+ }
updateJoints();
return true;
}
@@ -344,17 +351,26 @@ void SimDynamicsWindow::updateJointInfo()
info << "jv rn:" << tmp.toStdString();
qJV += QString(" / ");
- tmp = QString::number(dynamicsRobot->getJointAngle(rn),'f',3);
+ if (dynamicsRobot->isNodeActuated(rn))
+ tmp = QString::number(dynamicsRobot->getJointAngle(rn),'f',3);
+ else
+ tmp = QString("-");
qJV += tmp;
info << ",\tjv bul:" << tmp.toStdString();
qTarget = QString("Joint target: ");
- tmp = QString::number(dynamicsRobot->getNodeTarget(rn),'f',3);
+ if (dynamicsRobot->isNodeActuated(rn))
+ tmp = QString::number(dynamicsRobot->getNodeTarget(rn),'f',3);
+ else
+ tmp = QString("-");
qTarget +=tmp;
info << ",targ:" << tmp.toStdString();
qVel = QString("Joint velocity: ");
- tmp = QString::number(dynamicsRobot->getJointSpeed(rn),'f',3);
+ if (dynamicsRobot->isNodeActuated(rn))
+ tmp = QString::number(dynamicsRobot->getJointSpeed(rn),'f',3);
+ else
+ tmp = QString("-");
qVel +=tmp;
info << ",vel:" << tmp.toStdString();
Eigen::Matrix4f gp = rn->getGlobalPose();
@@ -381,8 +397,10 @@ void SimDynamicsWindow::updateJointInfo()
qVisu += QString::number(gp(1,3),'f',2);
qVisu += QString("/");
qVisu += QString::number(gp(2,3),'f',2);
-
- gp = dynamicsRobot->getComGlobal(rn);
+ if (dynamicsRobot->hasDynamicsRobotNode(rn))
+ gp = dynamicsRobot->getComGlobal(rn);
+ else
+ gp = Eigen::Matrix4f::Identity();
qCom = QString("COM (bullet):");
qCom += QString::number(gp(0,3),'f',2);
qCom += QString("/");
@@ -447,6 +465,7 @@ void SimDynamicsWindow::updateContactVisu()
std::vector<SimDynamics::DynamicsEngine::DynamicsContactInfo> c = dynamicsWorld->getEngine()->getContacts();
for (size_t i=0;i<c.size();i++)
{
+ cout << "Contact: " << c[i].objectA->getName() << " + " << c[i].objectB->getName() << endl;
SoSeparator *normal = new SoSeparator;
SoMatrixTransform *m = new SoMatrixTransform;
SbMatrix ma;
diff --git a/VirtualRobot/Nodes/RobotNode.cpp b/VirtualRobot/Nodes/RobotNode.cpp
index 73e58960e..b97cf6391 100644
--- a/VirtualRobot/Nodes/RobotNode.cpp
+++ b/VirtualRobot/Nodes/RobotNode.cpp
@@ -30,11 +30,13 @@ RobotNode::RobotNode( RobotWeakPtr rob,
CollisionModelPtr collisionModel,
float jointValueOffset,
const SceneObject::Physics &p,
- CollisionCheckerPtr colChecker)
+ CollisionCheckerPtr colChecker,
+ RobotNodeType type)
: SceneObject(name,visualization,collisionModel,p,colChecker)
{
+ nodeType = type;
maxVelocity = 0.0f;
maxAcceleration = 0.0f;
maxTorque = 0.0f;
@@ -82,9 +84,40 @@ bool RobotNode::initialize(SceneObjectPtr parent, const std::vector<SceneObjectP
v->setGlobalPose(postJointTransformation);
}
+ checkValidRobotNodeType();
+
return SceneObject::initialize(parent,children);
}
+void RobotNode::checkValidRobotNodeType()
+{
+ switch (nodeType)
+ {
+ case Generic:
+ return;
+ break;
+
+ case Joint:
+ THROW_VR_EXCEPTION_IF(visualizationModel, "No visualization models allowed in JointNodes");
+ THROW_VR_EXCEPTION_IF(collisionModel, "No collision models allowed in JointNodes");
+ THROW_VR_EXCEPTION_IF(postJointTransformation != Eigen::Matrix4f::Identity() , "No postJoint transformations allowed in JointNodes");
+ break;
+
+ case Body:
+ THROW_VR_EXCEPTION_IF(postJointTransformation != Eigen::Matrix4f::Identity() , "No transformations allowed in BodyNodes");
+ THROW_VR_EXCEPTION_IF(preJointTransformation != Eigen::Matrix4f::Identity() , "No transformations allowed in BodyNodes");
+
+ break;
+ case Transform:
+ THROW_VR_EXCEPTION_IF(visualizationModel, "No visualization models allowed in TransformationNodes");
+ THROW_VR_EXCEPTION_IF(collisionModel, "No collision models allowed in TransformationNodes");
+ break;
+ default:
+ VR_ERROR << "RobotNodeType nyi..." << endl;
+
+ }
+
+}
RobotPtr RobotNode::getRobot() const
{
@@ -585,5 +618,10 @@ Eigen::Matrix4f RobotNode::getGlobalPose() const
return globalPosePostJoint;
}
+RobotNode::RobotNodeType RobotNode::getType()
+{
+ return nodeType;
+}
+
} // namespace VirtualRobot
diff --git a/VirtualRobot/Nodes/RobotNode.h b/VirtualRobot/Nodes/RobotNode.h
index ef0001bf1..b00281850 100644
--- a/VirtualRobot/Nodes/RobotNode.h
+++ b/VirtualRobot/Nodes/RobotNode.h
@@ -58,7 +58,7 @@ class RobotNodeActuator;
*/
-class VIRTUAL_ROBOT_IMPORT_EXPORT RobotNode : /*public boost::enable_shared_from_this<RobotNode>,*/ public SceneObject
+class VIRTUAL_ROBOT_IMPORT_EXPORT RobotNode : public SceneObject
{
public:
friend class Robot;
@@ -68,6 +68,14 @@ public:
friend class RobotNodeActuator;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ enum RobotNodeType
+ {
+ Generic, //! No constraints
+ Joint, //! Only pre-joint transformations allowed (e.g. DH-theta for revolute joints). No visualization/collision models.
+ Body, //! No transformations allowed. Only visualization and/or collision models together with physic information.
+ Transform //! Only transformations allowed. No joint or model tags.
+ };
+
/*!
Constructor with settings.
*/
@@ -80,7 +88,8 @@ public:
CollisionModelPtr collisionModel = CollisionModelPtr(),
float jointValueOffset = 0.0f,
const SceneObject::Physics &p = SceneObject::Physics(),
- CollisionCheckerPtr colChecker = CollisionCheckerPtr());
+ CollisionCheckerPtr colChecker = CollisionCheckerPtr(),
+ RobotNodeType type = Generic);
/*!
*/
@@ -176,18 +185,6 @@ public:
*/
virtual void print(bool printChildren = false, bool printDecoration = true) const;
- //virtual void addChildNode(RobotNodePtr child);
-
- /*
- Returns true when child is a children of this node. If recursive is true, all children of children etc are also queried.
- */
- //virtual bool hasChildNode(const RobotNodePtr child, bool recursive = false) const;
- /*
- Returns true when child is a children of this node. If recursive is true, all children of children etc are also queried.
- */
- //virtual bool hasChildNode(const std::string &child, bool recursive = false) const;
-
-
float getJointLimitLo();
float getJointLimitHi();
@@ -259,13 +256,22 @@ public:
*/
float getMaxTorque();
+ RobotNodeType getType();
+
//! Forbid cloning method from SceneObject. We need to know the new robot for cloning
SceneObjectPtr clone( const std::string &name, CollisionCheckerPtr colChecker = CollisionCheckerPtr() ) const {THROW_VR_EXCEPTION("Cloning not allowed this way...");}
-private: // Use the private setters and getters instead
- //std::vector<std::string> childrenNames;
- //std::vector< RobotNodePtr > children;
- //RobotNodeWeakPtr parent;
+ /*!
+ When joint was created via DH parameters, they can be accessed here.
+ */
+ const DHParameter& getOptionalDHParameters() {return optionalDHParameter;}
+
+
+ /*!
+ Compute/Update the transformations of this joint and all child joints. Therefore the parent is queried for its pose.
+ This method is called by the robot in order to update the pose matrices.
+ */
+ virtual void updatePose(bool updateChildren = true);
protected:
@@ -281,13 +287,14 @@ protected:
*/
virtual void setJointValueNoUpdate(float q);
- /*!
- Compute/Update the transformations of this joint and all child joints. This method is called by the robot in order to update the pose matrices.
- */
- void updatePose(bool updateChildren = true);
+
+ /*!
+ Queries parent for global pose and updates visualization accordingly
+ */
+ virtual void updateTransformationMatrices();
/*!
- Update the pose of the robot
+ Update the pose according to parent
*/
virtual void updatePose( const Eigen::Matrix4f &parentPose, bool updateChildren = true );
@@ -303,6 +310,9 @@ protected:
virtual void updateVisualizationPose(const Eigen::Matrix4f &globalPose, bool updateChildren = false);
virtual void updateVisualizationPose(const Eigen::Matrix4f &globalPose, float jointValue, bool updateChildren = false);
+ //! Checks if nodeType constraints are fulfilled. Otherwise an exception is thrown. Called on initialization.
+ virtual void checkValidRobotNodeType();
+
///////////////////////// SETUP ////////////////////////////////////
RobotNode(){};
@@ -316,18 +326,19 @@ protected:
Eigen::Matrix4f postJointTransformation;
///////////////////////// SETUP ////////////////////////////////////
- virtual void updateTransformationMatrices();
virtual void updateTransformationMatrices(const Eigen::Matrix4f &parentPose);
RobotWeakPtr robot;
+ RobotNodeType nodeType;
+
- Eigen::Matrix4f globalPosePostJoint; //< The postJoint transformation applied to transformationJoint. Defines the starting pose for all child joints.
+ Eigen::Matrix4f globalPosePostJoint; //< The postJoint transformation applied to globalPose. Defines the starting pose for all child joints.
float jointValue; //< The joint value
/*!
Derived classes must implement their clone method here.
*/
- virtual RobotNodePtr _clone(const RobotPtr newRobot, /*const std::vector<std::string> newChildren,*/ const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker) = 0;
+ virtual RobotNodePtr _clone(const RobotPtr newRobot, const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker) = 0;
virtual SceneObject* _clone( const std::string &name, CollisionCheckerPtr colChecker = CollisionCheckerPtr() ) const {THROW_VR_EXCEPTION("Cloning not allowed this way...");}
diff --git a/VirtualRobot/Nodes/RobotNodeFactory.h b/VirtualRobot/Nodes/RobotNodeFactory.h
index 00183f743..6edcfcf3c 100644
--- a/VirtualRobot/Nodes/RobotNodeFactory.h
+++ b/VirtualRobot/Nodes/RobotNodeFactory.h
@@ -26,13 +26,14 @@
#include "../VirtualRobotImportExport.h"
#include "../AbstractFactoryMethod.h"
#include "../Transformation/DHParameter.h"
+#include "RobotNode.h"
#include <Eigen/Core>
#include <boost/shared_ptr.hpp>
+
namespace VirtualRobot
{
-class RobotNode;
class VIRTUAL_ROBOT_IMPORT_EXPORT RobotNodeFactory : public AbstractFactoryMethod<RobotNodeFactory, void*>
{
@@ -40,8 +41,8 @@ public:
RobotNodeFactory() {;}
virtual ~RobotNodeFactory() {;}
- virtual RobotNodePtr createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Matrix4f& postJointTransform, const Eigen::Vector3f& translationDirection, const SceneObject::Physics &p) const {return RobotNodePtr();}
- virtual RobotNodePtr createRobotNodeDH(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const DHParameter& dhParameters, const SceneObject::Physics &p) const {return RobotNodePtr();}
+ virtual RobotNodePtr createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Matrix4f& postJointTransform, const Eigen::Vector3f& translationDirection, const SceneObject::Physics &p = SceneObject::Physics(), RobotNode::RobotNodeType rntype = RobotNode::Generic) const {return RobotNodePtr();}
+ virtual RobotNodePtr createRobotNodeDH(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const DHParameter& dhParameters, const SceneObject::Physics &p = SceneObject::Physics(), RobotNode::RobotNodeType rntype = RobotNode::Generic ) const {return RobotNodePtr();}
};
} // namespace VirtualRobot
diff --git a/VirtualRobot/Nodes/RobotNodeFixed.cpp b/VirtualRobot/Nodes/RobotNodeFixed.cpp
index 3285f986b..8a1a8b88f 100644
--- a/VirtualRobot/Nodes/RobotNodeFixed.cpp
+++ b/VirtualRobot/Nodes/RobotNodeFixed.cpp
@@ -17,12 +17,14 @@ RobotNodeFixed::RobotNodeFixed(RobotWeakPtr rob,
VisualizationNodePtr visualization,
CollisionModelPtr collisionModel,
const SceneObject::Physics &p,
- CollisionCheckerPtr colChecker
- ) : RobotNode(rob,name,0.0f,0.0f,visualization,collisionModel,0.0f,p,colChecker)
+ CollisionCheckerPtr colChecker,
+ RobotNodeType type
+ ) : RobotNode(rob,name,0.0f,0.0f,visualization,collisionModel,0.0f,p,colChecker,type)
{
optionalDHParameter.isSet = false;
this->preJointTransformation = preJointTransform;
this->postJointTransformation = postJointTransform;
+ checkValidRobotNodeType();
}
RobotNodeFixed::RobotNodeFixed(RobotWeakPtr rob,
@@ -31,8 +33,9 @@ RobotNodeFixed::RobotNodeFixed(RobotWeakPtr rob,
VisualizationNodePtr visualization,
CollisionModelPtr collisionModel,
const SceneObject::Physics &p,
- CollisionCheckerPtr colChecker
- ) : RobotNode(rob,name,0.0f,1.0f,visualization,collisionModel,0.0f,p,colChecker)
+ CollisionCheckerPtr colChecker,
+ RobotNodeType type
+ ) : RobotNode(rob,name,0.0f,1.0f,visualization,collisionModel,0.0f,p,colChecker,type)
{
initialized = false;
optionalDHParameter.isSet = true;
@@ -100,12 +103,18 @@ RobotNodePtr RobotNodeFixed::_clone(const RobotPtr newRobot, /*const std::vector
RobotNodePtr result;
if (optionalDHParameter.isSet)
- result.reset(new RobotNodeFixed(newRobot,name,optionalDHParameter.aMM(),optionalDHParameter.dMM(), optionalDHParameter.alphaRadian(), optionalDHParameter.thetaRadian(),visualizationModel,collisionModel,physics,colChecker));
+ result.reset(new RobotNodeFixed(newRobot,name,optionalDHParameter.aMM(),optionalDHParameter.dMM(), optionalDHParameter.alphaRadian(), optionalDHParameter.thetaRadian(),visualizationModel,collisionModel,physics,colChecker,nodeType));
else
- result.reset(new RobotNodeFixed(newRobot,name,getPreJointTransformation(),getPostJointTransformation(),visualizationModel,collisionModel,physics,colChecker));
+ result.reset(new RobotNodeFixed(newRobot,name,getPreJointTransformation(),getPostJointTransformation(),visualizationModel,collisionModel,physics,colChecker,nodeType));
return result;
}
+void RobotNodeFixed::checkValidRobotNodeType()
+{
+ RobotNode::checkValidRobotNodeType();
+ THROW_VR_EXCEPTION_IF (nodeType==Joint, "RobotNodeFixed not compatible with JointNode");
+}
+
} // namespace
diff --git a/VirtualRobot/Nodes/RobotNodeFixed.h b/VirtualRobot/Nodes/RobotNodeFixed.h
index caa0186e6..937741dab 100644
--- a/VirtualRobot/Nodes/RobotNodeFixed.h
+++ b/VirtualRobot/Nodes/RobotNodeFixed.h
@@ -53,8 +53,8 @@ public:
VisualizationNodePtr visualization = VisualizationNodePtr(),//!< A visualization model
CollisionModelPtr collisionModel = CollisionModelPtr(), //!< A collision model
const SceneObject::Physics &p = SceneObject::Physics(), //!< physics information
- CollisionCheckerPtr colChecker = CollisionCheckerPtr() //!< A collision checker instance (if not set, the global col checker is used)
- );
+ CollisionCheckerPtr colChecker = CollisionCheckerPtr(), //!< A collision checker instance (if not set, the global col checker is used)
+ RobotNodeType type = Generic);
/*!
Initialize with DH parameters.
@@ -73,8 +73,8 @@ public:
VisualizationNodePtr visualization = VisualizationNodePtr(), //!< A visualization model
CollisionModelPtr collisionModel = CollisionModelPtr(), //!< A collision model
const SceneObject::Physics &p = SceneObject::Physics(), //!< physics information
- CollisionCheckerPtr colChecker = CollisionCheckerPtr() //!< A collision checker instance (if not set, the global col checker is used)
- );
+ CollisionCheckerPtr colChecker = CollisionCheckerPtr(), //!< A collision checker instance (if not set, the global col checker is used)
+ RobotNodeType type = Generic);
/*!
*/
@@ -88,6 +88,8 @@ public:
virtual void print(bool printChildren = false, bool printDecoration = true) const;
protected:
+ //! Checks if nodeType constraints are fulfilled. Otherwise an exception is thrown. Called on initialization.
+ virtual void checkValidRobotNodeType();
RobotNodeFixed(){};
virtual void updateTransformationMatrices(const Eigen::Matrix4f &parentPose);
diff --git a/VirtualRobot/Nodes/RobotNodeFixedFactory.cpp b/VirtualRobot/Nodes/RobotNodeFixedFactory.cpp
index df8ae3eea..212e4e9d6 100644
--- a/VirtualRobot/Nodes/RobotNodeFixedFactory.cpp
+++ b/VirtualRobot/Nodes/RobotNodeFixedFactory.cpp
@@ -26,9 +26,9 @@ RobotNodeFixedFactory::~RobotNodeFixedFactory()
*
* \return instance of VirtualRobot::RobotNodeFixed.
*/
-RobotNodePtr RobotNodeFixedFactory::createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Matrix4f& postJointTransform, const Eigen::Vector3f& translationDirection, const SceneObject::Physics &p) const
+RobotNodePtr RobotNodeFixedFactory::createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Matrix4f& postJointTransform, const Eigen::Vector3f& translationDirection, const SceneObject::Physics &p, RobotNode::RobotNodeType rntype) const
{
- RobotNodePtr robotNode(new RobotNodeFixed(robot, nodeName, preJointTransform, postJointTransform, visualizationModel, collisionModel, p));
+ RobotNodePtr robotNode(new RobotNodeFixed(robot, nodeName, preJointTransform, postJointTransform, visualizationModel, collisionModel, p, CollisionCheckerPtr(), rntype));
return robotNode;
}
@@ -39,9 +39,9 @@ RobotNodePtr RobotNodeFixedFactory::createRobotNode(RobotPtr robot, const std::s
*
* \return instance of VirtualRobot::RobotNodeFixed.
*/
-RobotNodePtr RobotNodeFixedFactory::createRobotNodeDH(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const DHParameter& dhParameters, const SceneObject::Physics &p) const
+RobotNodePtr RobotNodeFixedFactory::createRobotNodeDH(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const DHParameter& dhParameters, const SceneObject::Physics &p, RobotNode::RobotNodeType rntype) const
{
- RobotNodePtr robotNode(new RobotNodeFixed(robot, nodeName,dhParameters.aMM(), dhParameters.dMM(), dhParameters.alphaRadian(), dhParameters.thetaRadian(), visualizationModel, collisionModel, p));
+ RobotNodePtr robotNode(new RobotNodeFixed(robot, nodeName,dhParameters.aMM(), dhParameters.dMM(), dhParameters.alphaRadian(), dhParameters.thetaRadian(), visualizationModel, collisionModel, p, CollisionCheckerPtr(), rntype));
return robotNode;
/*
diff --git a/VirtualRobot/Nodes/RobotNodeFixedFactory.h b/VirtualRobot/Nodes/RobotNodeFixedFactory.h
index 6331096f6..89b466fdb 100644
--- a/VirtualRobot/Nodes/RobotNodeFixedFactory.h
+++ b/VirtualRobot/Nodes/RobotNodeFixedFactory.h
@@ -40,8 +40,8 @@ public:
RobotNodeFixedFactory();
virtual ~RobotNodeFixedFactory();
- virtual RobotNodePtr createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Matrix4f& postJointTransform, const Eigen::Vector3f& translationDirection, const SceneObject::Physics &p = SceneObject::Physics()) const;
- virtual RobotNodePtr createRobotNodeDH(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const DHParameter& dhParameters, const SceneObject::Physics &p = SceneObject::Physics()) const;
+ virtual RobotNodePtr createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Matrix4f& postJointTransform, const Eigen::Vector3f& translationDirection, const SceneObject::Physics &p = SceneObject::Physics(), RobotNode::RobotNodeType rntype = RobotNode::Generic) const;
+ virtual RobotNodePtr createRobotNodeDH(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const DHParameter& dhParameters, const SceneObject::Physics &p = SceneObject::Physics(), RobotNode::RobotNodeType rntype = RobotNode::Generic) const;
// AbstractFactoryMethod
public:
static std::string getName();
diff --git a/VirtualRobot/Nodes/RobotNodePrismatic.cpp b/VirtualRobot/Nodes/RobotNodePrismatic.cpp
index 881c204f6..dd3eb411b 100644
--- a/VirtualRobot/Nodes/RobotNodePrismatic.cpp
+++ b/VirtualRobot/Nodes/RobotNodePrismatic.cpp
@@ -20,17 +20,16 @@ RobotNodePrismatic::RobotNodePrismatic(RobotWeakPtr rob,
CollisionModelPtr collisionModel,
float jointValueOffset,
const SceneObject::Physics &p,
- CollisionCheckerPtr colChecker
- ) : RobotNode(rob,name,jointLimitLo,jointLimitHi,visualization,collisionModel,jointValueOffset,p,colChecker)
-
+ CollisionCheckerPtr colChecker,
+ RobotNodeType type
+ ) : RobotNode(rob,name,jointLimitLo,jointLimitHi,visualization,collisionModel,jointValueOffset,p,colChecker,type)
{
initialized = false;
optionalDHParameter.isSet = false;
this->preJointTransformation = preJointTransform;
this->postJointTransformation = postJointTransform;
- //this->setPreJointTransformation(preJointTransform);
this->jointTranslationDirection = translationDirection;
- //this->setPostJointTransformation(postJointTransform);
+ checkValidRobotNodeType();
}
RobotNodePrismatic::RobotNodePrismatic(RobotWeakPtr rob,
@@ -42,9 +41,9 @@ RobotNodePrismatic::RobotNodePrismatic(RobotWeakPtr rob,
CollisionModelPtr collisionModel,
float jointValueOffset,
const SceneObject::Physics &p,
- CollisionCheckerPtr colChecker
- ) : RobotNode(rob,name,jointLimitLo,jointLimitHi,visualization,collisionModel,jointValueOffset,p,colChecker)
-
+ CollisionCheckerPtr colChecker,
+ RobotNodeType type
+ ) : RobotNode(rob,name,jointLimitLo,jointLimitHi,visualization,collisionModel,jointValueOffset,p,colChecker,type)
{
initialized = false;
optionalDHParameter.isSet = true;
@@ -70,11 +69,12 @@ RobotNodePrismatic::RobotNodePrismatic(RobotWeakPtr rob,
RotAlpha(2,2) = cos(alpha);
// fixed rotation around theta
- this->preJointTransformation = RotTheta;
+ this->preJointTransformation = RotTheta*TransD;
// joint setup
jointTranslationDirection = Eigen::Vector3f(0,0,1); // translation along the z axis
// compute postJointTransformation
- this->postJointTransformation = TransD*TransA*RotAlpha;
+ this->postJointTransformation = TransA*RotAlpha;
+ checkValidRobotNodeType();
}
RobotNodePrismatic::~RobotNodePrismatic()
@@ -120,9 +120,9 @@ RobotNodePtr RobotNodePrismatic::_clone(const RobotPtr newRobot, /*const std::ve
ReadLockPtr lock = getRobot()->getReadLock();
if (optionalDHParameter.isSet)
- result.reset(new RobotNodePrismatic(newRobot,name, jointLimitLo,jointLimitHi,optionalDHParameter.aMM(),optionalDHParameter.dMM(), optionalDHParameter.alphaRadian(), optionalDHParameter.thetaRadian(),visualizationModel,collisionModel, jointValueOffset,physics,colChecker));
+ result.reset(new RobotNodePrismatic(newRobot,name, jointLimitLo,jointLimitHi,optionalDHParameter.aMM(),optionalDHParameter.dMM(), optionalDHParameter.alphaRadian(), optionalDHParameter.thetaRadian(),visualizationModel,collisionModel, jointValueOffset,physics,colChecker,nodeType));
else
- result.reset(new RobotNodePrismatic(newRobot,name,jointLimitLo,jointLimitHi,getPreJointTransformation(),jointTranslationDirection,getPostJointTransformation(),visualizationModel,collisionModel,jointValueOffset,physics,colChecker));
+ result.reset(new RobotNodePrismatic(newRobot,name,jointLimitLo,jointLimitHi,getPreJointTransformation(),jointTranslationDirection,getPostJointTransformation(),visualizationModel,collisionModel,jointValueOffset,physics,colChecker,nodeType));
return result;
}
@@ -184,6 +184,12 @@ Eigen::Vector3f RobotNodePrismatic::getJointTranslationDirectionJointCoordSystem
return jointTranslationDirection;
}
+void RobotNodePrismatic::checkValidRobotNodeType()
+{
+ RobotNode::checkValidRobotNodeType();
+ THROW_VR_EXCEPTION_IF (nodeType==Body || nodeType==Transform, "RobotNodePrismatic must be a JointNode or a GenericNode");
+}
+
} // namespace
diff --git a/VirtualRobot/Nodes/RobotNodePrismatic.h b/VirtualRobot/Nodes/RobotNodePrismatic.h
index 01f06e6e1..956e8635c 100644
--- a/VirtualRobot/Nodes/RobotNodePrismatic.h
+++ b/VirtualRobot/Nodes/RobotNodePrismatic.h
@@ -58,8 +58,8 @@ public:
CollisionModelPtr collisionModel = CollisionModelPtr(), //!< A collision model
float jointValueOffset = 0.0f, //!< An offset that is internally added to the joint value
const SceneObject::Physics &p = SceneObject::Physics(), //!< physics information
- CollisionCheckerPtr colChecker = CollisionCheckerPtr() //!< A collision checker instance (if not set, the global col checker is used)
- );
+ CollisionCheckerPtr colChecker = CollisionCheckerPtr(), //!< A collision checker instance (if not set, the global col checker is used)
+ RobotNodeType type = Generic);
RobotNodePrismatic( RobotWeakPtr rob, //!< The robot
const std::string &name, //!< The name
float jointLimitLo, //!< lower joint limit
@@ -72,8 +72,8 @@ public:
CollisionModelPtr collisionModel = CollisionModelPtr(), //!< A collision model
float jointValueOffset = 0.0f, //!< An offset that is internally added to the joint value
const SceneObject::Physics &p = SceneObject::Physics(), //!< physics information
- CollisionCheckerPtr colChecker = CollisionCheckerPtr() //!< A collision checker instance (if not set, the global col checker is used)
- );
+ CollisionCheckerPtr colChecker = CollisionCheckerPtr(), //!< A collision checker instance (if not set, the global col checker is used)
+ RobotNodeType type = Generic);
/*!
*/
virtual ~RobotNodePrismatic();
@@ -107,6 +107,9 @@ protected:
*/
virtual void updateVisualizationPose(const Eigen::Matrix4f &globalPose, bool updateChildren = false);
+ //! Checks if nodeType constraints are fulfilled. Otherwise an exception is thrown. Called on initialization.
+ virtual void checkValidRobotNodeType();
+
RobotNodePrismatic(){};
virtual void updateTransformationMatrices(const Eigen::Matrix4f &parentPose);
diff --git a/VirtualRobot/Nodes/RobotNodePrismaticFactory.cpp b/VirtualRobot/Nodes/RobotNodePrismaticFactory.cpp
index 445c4e2ea..d488ca9f9 100644
--- a/VirtualRobot/Nodes/RobotNodePrismaticFactory.cpp
+++ b/VirtualRobot/Nodes/RobotNodePrismaticFactory.cpp
@@ -26,9 +26,9 @@ RobotNodePrismaticFactory::~RobotNodePrismaticFactory()
*
* \return instance of VirtualRobot::RobotNodePrismatic.
*/
-RobotNodePtr RobotNodePrismaticFactory::createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Matrix4f& postJointTransform, const Eigen::Vector3f& translationDirection, const SceneObject::Physics &p) const
+RobotNodePtr RobotNodePrismaticFactory::createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Matrix4f& postJointTransform, const Eigen::Vector3f& translationDirection, const SceneObject::Physics &p, RobotNode::RobotNodeType rntype) const
{
- RobotNodePtr robotNode(new RobotNodePrismatic(robot, nodeName, limitLow, limitHigh, preJointTransform, translationDirection, postJointTransform, visualizationModel, collisionModel, jointValueOffset,p));
+ RobotNodePtr robotNode(new RobotNodePrismatic(robot, nodeName, limitLow, limitHigh, preJointTransform, translationDirection, postJointTransform, visualizationModel, collisionModel, jointValueOffset,p,CollisionCheckerPtr(), rntype));
return robotNode;
}
@@ -39,9 +39,9 @@ RobotNodePtr RobotNodePrismaticFactory::createRobotNode(RobotPtr robot, const st
*
* \return instance of VirtualRobot::RobotNodePrismatic.
*/
-RobotNodePtr RobotNodePrismaticFactory::createRobotNodeDH(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const DHParameter& dhParameters, const SceneObject::Physics &p) const
+RobotNodePtr RobotNodePrismaticFactory::createRobotNodeDH(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const DHParameter& dhParameters, const SceneObject::Physics &p, RobotNode::RobotNodeType rntype) const
{
- RobotNodePtr robotNode(new RobotNodePrismatic(robot, nodeName, limitLow, limitHigh, dhParameters.aMM(), dhParameters.dMM(), dhParameters.alphaRadian(), dhParameters.thetaRadian(), visualizationModel, collisionModel, jointValueOffset,p));
+ RobotNodePtr robotNode(new RobotNodePrismatic(robot, nodeName, limitLow, limitHigh, dhParameters.aMM(), dhParameters.dMM(), dhParameters.alphaRadian(), dhParameters.thetaRadian(), visualizationModel, collisionModel, jointValueOffset,p,CollisionCheckerPtr(), rntype));
return robotNode;
}
diff --git a/VirtualRobot/Nodes/RobotNodePrismaticFactory.h b/VirtualRobot/Nodes/RobotNodePrismaticFactory.h
index db3a2d5d1..5081a8007 100644
--- a/VirtualRobot/Nodes/RobotNodePrismaticFactory.h
+++ b/VirtualRobot/Nodes/RobotNodePrismaticFactory.h
@@ -40,8 +40,8 @@ public:
RobotNodePrismaticFactory();
virtual ~RobotNodePrismaticFactory();
- virtual RobotNodePtr createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Matrix4f& postJointTransform, const Eigen::Vector3f& translationDirection, const SceneObject::Physics &p = SceneObject::Physics()) const;
- virtual RobotNodePtr createRobotNodeDH(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const DHParameter& dhParameters, const SceneObject::Physics &p = SceneObject::Physics()) const;
+ virtual RobotNodePtr createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Matrix4f& postJointTransform, const Eigen::Vector3f& translationDirection, const SceneObject::Physics &p = SceneObject::Physics(), RobotNode::RobotNodeType rntype = RobotNode::Generic) const;
+ virtual RobotNodePtr createRobotNodeDH(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const DHParameter& dhParameters, const SceneObject::Physics &p = SceneObject::Physics(), RobotNode::RobotNodeType rntype = RobotNode::Generic) const;
// AbstractFactoryMethod
public:
static std::string getName();
diff --git a/VirtualRobot/Nodes/RobotNodeRevolute.cpp b/VirtualRobot/Nodes/RobotNodeRevolute.cpp
index d5a6c8aa3..db810c57c 100644
--- a/VirtualRobot/Nodes/RobotNodeRevolute.cpp
+++ b/VirtualRobot/Nodes/RobotNodeRevolute.cpp
@@ -21,15 +21,16 @@ RobotNodeRevolute::RobotNodeRevolute(RobotWeakPtr rob,
CollisionModelPtr collisionModel,
float jointValueOffset,
const SceneObject::Physics &p,
- CollisionCheckerPtr colChecker
- ) : RobotNode(rob,name,jointLimitLo,jointLimitHi,visualization,collisionModel,jointValueOffset,p,colChecker)
-
+ CollisionCheckerPtr colChecker,
+ RobotNodeType type
+ ) : RobotNode(rob,name,jointLimitLo,jointLimitHi,visualization,collisionModel,jointValueOffset,p,colChecker,type)
{
initialized = false;
optionalDHParameter.isSet = false;
this->preJointTransformation = preJointTransform;
this->jointRotationAxis = axis;
this->postJointTransformation = postJointTransform;
+ checkValidRobotNodeType();
}
RobotNodeRevolute::RobotNodeRevolute(RobotWeakPtr rob,
@@ -41,8 +42,9 @@ RobotNodeRevolute::RobotNodeRevolute(RobotWeakPtr rob,
CollisionModelPtr collisionModel,
float jointValueOffset,
const SceneObject::Physics &p,
- CollisionCheckerPtr colChecker
- ) : RobotNode(rob,name,jointLimitLo,jointLimitHi,visualization,collisionModel,jointValueOffset,p,colChecker)
+ CollisionCheckerPtr colChecker,
+ RobotNodeType type
+ ) : RobotNode(rob,name,jointLimitLo,jointLimitHi,visualization,collisionModel,jointValueOffset,p,colChecker,type)
{
initialized = false;
@@ -68,9 +70,10 @@ RobotNodeRevolute::RobotNodeRevolute(RobotWeakPtr rob,
RotAlpha(2,1) = sin(alpha);
RotAlpha(2,2) = cos(alpha);
- this->preJointTransformation = RotTheta;//Eigen::Matrix4f::Identity(); // no pre transformation->why?
+ this->preJointTransformation = RotTheta;
this->jointRotationAxis = Eigen::Vector3f(0,0,1); // rotation around z axis
this->postJointTransformation = TransD*TransA*RotAlpha;
+ checkValidRobotNodeType();
}
@@ -115,9 +118,9 @@ RobotNodePtr RobotNodeRevolute::_clone(const RobotPtr newRobot, /*const std::vec
RobotNodePtr result;
if (optionalDHParameter.isSet)
- result.reset(new RobotNodeRevolute(newRobot,name, jointLimitLo,jointLimitHi,optionalDHParameter.aMM(),optionalDHParameter.dMM(), optionalDHParameter.alphaRadian(), optionalDHParameter.thetaRadian(),visualizationModel,collisionModel, jointValueOffset,physics,colChecker));
+ result.reset(new RobotNodeRevolute(newRobot,name, jointLimitLo,jointLimitHi,optionalDHParameter.aMM(),optionalDHParameter.dMM(), optionalDHParameter.alphaRadian(), optionalDHParameter.thetaRadian(),visualizationModel,collisionModel, jointValueOffset,physics,colChecker,nodeType));
else
- result.reset(new RobotNodeRevolute(newRobot,name,jointLimitLo,jointLimitHi,getPreJointTransformation(),jointRotationAxis,getPostJointTransformation(),visualizationModel,collisionModel,jointValueOffset,physics,colChecker));
+ result.reset(new RobotNodeRevolute(newRobot,name,jointLimitLo,jointLimitHi,getPreJointTransformation(),jointRotationAxis,getPostJointTransformation(),visualizationModel,collisionModel,jointValueOffset,physics,colChecker,nodeType));
return result;
}
@@ -179,4 +182,10 @@ Eigen::Vector3f RobotNodeRevolute::getJointRotationAxisInJointCoordSystem() cons
return jointRotationAxis;
}
+void RobotNodeRevolute::checkValidRobotNodeType()
+{
+ RobotNode::checkValidRobotNodeType();
+ THROW_VR_EXCEPTION_IF (nodeType==Body || nodeType==Transform, "RobotNodeRevolute must be a JointNode or a GenericNode");
+}
+
} // namespace
diff --git a/VirtualRobot/Nodes/RobotNodeRevolute.h b/VirtualRobot/Nodes/RobotNodeRevolute.h
index 1ece8d072..fcca1098d 100644
--- a/VirtualRobot/Nodes/RobotNodeRevolute.h
+++ b/VirtualRobot/Nodes/RobotNodeRevolute.h
@@ -59,8 +59,8 @@ public:
CollisionModelPtr collisionModel = CollisionModelPtr(), //!< A collision model
float jointValueOffset = 0.0f, //!< An offset that is internally added to the joint value
const SceneObject::Physics &p = SceneObject::Physics(), //!< physics information
- CollisionCheckerPtr colChecker = CollisionCheckerPtr() //!< A collision checker instance (if not set, the global col checker is used)
- );
+ CollisionCheckerPtr colChecker = CollisionCheckerPtr(), //!< A collision checker instance (if not set, the global col checker is used)
+ RobotNodeType type = Generic);
RobotNodeRevolute( RobotWeakPtr rob, //!< The robot
const std::string &name, //!< The name
float jointLimitLo, //!< lower joint limit
@@ -73,8 +73,8 @@ public:
CollisionModelPtr collisionModel = CollisionModelPtr(), //!< A collision model
float jointValueOffset = 0.0f, //!< An offset that is internally added to the joint value
const SceneObject::Physics &p = SceneObject::Physics(), //!< physics information
- CollisionCheckerPtr colChecker = CollisionCheckerPtr() //!< A collision checker instance (if not set, the global col checker is used)
- );
+ CollisionCheckerPtr colChecker = CollisionCheckerPtr(), //!< A collision checker instance (if not set, the global col checker is used)
+ RobotNodeType type = Generic);
/*!
*/
virtual ~RobotNodeRevolute();
@@ -106,6 +106,8 @@ protected:
*/
virtual void updateVisualizationPose(const Eigen::Matrix4f &globalPose, bool updateChildren = false);
+ //! Checks if nodeType constraints are fulfilled. Otherwise an exception is thrown. Called on initialization.
+ virtual void checkValidRobotNodeType();
RobotNodeRevolute(){};
diff --git a/VirtualRobot/Nodes/RobotNodeRevoluteFactory.cpp b/VirtualRobot/Nodes/RobotNodeRevoluteFactory.cpp
index 3c9a7cd1f..1cb0d19ac 100644
--- a/VirtualRobot/Nodes/RobotNodeRevoluteFactory.cpp
+++ b/VirtualRobot/Nodes/RobotNodeRevoluteFactory.cpp
@@ -26,9 +26,9 @@ RobotNodeRevoluteFactory::~RobotNodeRevoluteFactory()
*
* \return instance of VirtualRobot::RobotNodeRevolute.
*/
-RobotNodePtr RobotNodeRevoluteFactory::createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Matrix4f& postJointTransform, const Eigen::Vector3f& translationDirection, const SceneObject::Physics &p) const
+RobotNodePtr RobotNodeRevoluteFactory::createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Matrix4f& postJointTransform, const Eigen::Vector3f& translationDirection, const SceneObject::Physics &p, RobotNode::RobotNodeType rntype) const
{
- RobotNodePtr robotNode(new RobotNodeRevolute(robot, nodeName, limitLow, limitHigh, preJointTransform, axis, postJointTransform, visualizationModel, collisionModel, jointValueOffset, p));
+ RobotNodePtr robotNode(new RobotNodeRevolute(robot, nodeName, limitLow, limitHigh, preJointTransform, axis, postJointTransform, visualizationModel, collisionModel, jointValueOffset, p, CollisionCheckerPtr(), rntype));
return robotNode;
}
@@ -39,9 +39,9 @@ RobotNodePtr RobotNodeRevoluteFactory::createRobotNode(RobotPtr robot, const std
*
* \return instance of VirtualRobot::RobotNodeRevolute.
*/
-RobotNodePtr RobotNodeRevoluteFactory::createRobotNodeDH(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const DHParameter& dhParameters, const SceneObject::Physics &p) const
+RobotNodePtr RobotNodeRevoluteFactory::createRobotNodeDH(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const DHParameter& dhParameters, const SceneObject::Physics &p, RobotNode::RobotNodeType rntype) const
{
- RobotNodePtr robotNode(new RobotNodeRevolute(robot, nodeName, limitLow, limitHigh, dhParameters.aMM(), dhParameters.dMM(), dhParameters.alphaRadian(), dhParameters.thetaRadian(), visualizationModel, collisionModel, jointValueOffset, p));
+ RobotNodePtr robotNode(new RobotNodeRevolute(robot, nodeName, limitLow, limitHigh, dhParameters.aMM(), dhParameters.dMM(), dhParameters.alphaRadian(), dhParameters.thetaRadian(), visualizationModel, collisionModel, jointValueOffset, p, CollisionCheckerPtr(), rntype));
return robotNode;
}
diff --git a/VirtualRobot/Nodes/RobotNodeRevoluteFactory.h b/VirtualRobot/Nodes/RobotNodeRevoluteFactory.h
index db77e5882..793c8e574 100644
--- a/VirtualRobot/Nodes/RobotNodeRevoluteFactory.h
+++ b/VirtualRobot/Nodes/RobotNodeRevoluteFactory.h
@@ -38,8 +38,8 @@ public:
RobotNodeRevoluteFactory();
virtual ~RobotNodeRevoluteFactory();
- virtual RobotNodePtr createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Matrix4f& postJointTransform, const Eigen::Vector3f& translationDirection, const SceneObject::Physics &p = SceneObject::Physics()) const;
- virtual RobotNodePtr createRobotNodeDH(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const DHParameter& dhParameters, const SceneObject::Physics &p = SceneObject::Physics()) const;
+ virtual RobotNodePtr createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Matrix4f& postJointTransform, const Eigen::Vector3f& translationDirection, const SceneObject::Physics &p = SceneObject::Physics(), RobotNode::RobotNodeType rntype = RobotNode::Generic) const;
+ virtual RobotNodePtr createRobotNodeDH(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const DHParameter& dhParameters, const SceneObject::Physics &p = SceneObject::Physics(), RobotNode::RobotNodeType rntype = RobotNode::Generic) const;
// AbstractFactoryMethod
public:
static std::string getName();
diff --git a/VirtualRobot/SceneObject.cpp b/VirtualRobot/SceneObject.cpp
index 47ff473b7..985139f05 100644
--- a/VirtualRobot/SceneObject.cpp
+++ b/VirtualRobot/SceneObject.cpp
@@ -634,4 +634,10 @@ void SceneObject::attached( SceneObjectPtr parent )
}
+
+std::vector<std::string> SceneObject::getIgnoredCollisionModels()
+{
+ return physics.ignoreCollisions;
+}
+
} // namespace
diff --git a/VirtualRobot/SceneObject.h b/VirtualRobot/SceneObject.h
index 886260009..248138d7e 100644
--- a/VirtualRobot/SceneObject.h
+++ b/VirtualRobot/SceneObject.h
@@ -70,11 +70,17 @@ public:
std::cout << " ** Mass: " << massKg << " [kg]" << std::endl;
std::cout << " ** local CoM [mm]: " << localCoM(0) << localCoM(1) << localCoM(2) << std::endl;
std::cout << " ** inertia matrix [kg*m^2] :\n " << intertiaMatrix << std::endl;
+ if (ignoreCollisions.size()>0)
+ {
+ std::cout << " ** Ignore Collisions with:" << std::endl;
+ for (size_t i=0;i<ignoreCollisions.size();i++)
+ std::cout << " **** " << ignoreCollisions[i] << std::endl;
+ }
}
bool isSet()
{
- return (massKg!=0.0f || comLocation!=eCustom || !localCoM.isZero() || !intertiaMatrix.isIdentity());
+ return (massKg!=0.0f || comLocation!=eCustom || !localCoM.isZero() || !intertiaMatrix.isIdentity() || ignoreCollisions.size()>0);
}
std::string getXMLString(int tabs)
@@ -95,6 +101,8 @@ public:
ss << ta << "\t<InertiaMatrix>\n";
ss << MathTools::getTransformXMLString(intertiaMatrix,tabs+2,true);
ss << ta << "\t</InertiaMatrix>\n";
+ for (size_t i=0;i<ignoreCollisions.size();i++)
+ ss << ta << "\t<IgnoreCollisions name='" << ignoreCollisions[i] << "'/>\n";
ss << ta << "</Physics>\n";
return ss.str();
}
@@ -103,6 +111,7 @@ public:
float massKg; //!< The mass of this object
CoMLocation comLocation; //!< Where is the CoM located
Eigen::Matrix3f intertiaMatrix; //! in kg*m^2
+ std::vector< std::string > ignoreCollisions; // ignore collisions with other objects (only used within collision engines)
};
/*!
@@ -268,6 +277,11 @@ public:
void setInertiaMatrix(const Eigen::Matrix3f &im);
+ /*!
+ Collisions with these models are ignored by physics engine (only considered within the SimDynamics package!).
+ */
+ std::vector<std::string> getIgnoredCollisionModels();
+
virtual void print(bool printChildren = false, bool printDecoration = true) const;
@@ -330,6 +344,9 @@ public:
virtual std::vector<SceneObjectPtr> getChildren() const {return children;}
+ //! Compute the global pose of this object
+ virtual void updatePose( bool updateChildren = true);
+
protected:
virtual SceneObject* _clone( const std::string &name, CollisionCheckerPtr colChecker = CollisionCheckerPtr() ) const;
@@ -338,8 +355,6 @@ protected:
//! Parent attached this object
virtual void attached(SceneObjectPtr parent);
- //! Compute the global pose of this object
- virtual void updatePose( bool updateChildren = true);
virtual void updatePose( const Eigen::Matrix4f &parentPose, bool updateChildren = true );
diff --git a/VirtualRobot/Transformation/DHParameter.h b/VirtualRobot/Transformation/DHParameter.h
index 5d8a265d0..33b808c01 100644
--- a/VirtualRobot/Transformation/DHParameter.h
+++ b/VirtualRobot/Transformation/DHParameter.h
@@ -36,6 +36,10 @@ public:
DHParameter()
{
_a = _d = _alpha = _theta = 0;
+ _thetaRotation = Eigen::Matrix4f::Identity();
+ _dTranslation = Eigen::Matrix4f::Identity();
+ _aTranslation = Eigen::Matrix4f::Identity();
+ _alphaRotation = Eigen::Matrix4f::Identity();
isSet = false;
};
@@ -78,6 +82,14 @@ public:
return _alphaRotation;
}
+ //! The complete transformation
+ Eigen::Matrix4f transformation() const
+ {
+ return _thetaRotation*_dTranslation*_aTranslation*_alphaRotation;
+ }
+
+
+
bool isSet;
protected:
void updateTransformations()
diff --git a/VirtualRobot/XML/BaseIO.cpp b/VirtualRobot/XML/BaseIO.cpp
index 0a7c32c29..fe0393569 100644
--- a/VirtualRobot/XML/BaseIO.cpp
+++ b/VirtualRobot/XML/BaseIO.cpp
@@ -290,6 +290,51 @@ void BaseIO::processTransformNode(rapidxml::xml_node<char> *transformXMLNode, co
}
}
}
+
+ // DH
+ rapidxml::xml_node<> *dhXMLNode = trXMLNode->first_node("dh",0,false);
+ THROW_VR_EXCEPTION_IF((dhXMLNode && (rotation||translation)), "Multiple rotations/translations defined in <Transformation> tag: " << tagName << "! Ignoring DH node." << endl);
+
+ if (dhXMLNode)
+ {
+ DHParameter dh;
+ processDHNode(dhXMLNode,dh);
+ transform = dh.transformation();
+ rotation = true;
+ translation = true;
+ }
+}
+
+
+void BaseIO::processDHNode(rapidxml::xml_node<char> *dhXMLNode, DHParameter &dh)
+{
+ rapidxml::xml_attribute<> *attr;
+ std::vector< Units > unitsAttr = getUnitsAttributes(dhXMLNode);
+ Units uAngle("rad");
+ Units uLength("mm");
+ for (size_t i=0;i<unitsAttr.size();i++)
+ {
+ if (unitsAttr[i].isAngle())
+ uAngle = unitsAttr[i];
+ if (unitsAttr[i].isLength())
+ uLength = unitsAttr[i];
+ }
+
+ dh.isSet = true;
+ bool isRadian = uAngle.isRadian();
+
+ attr = dhXMLNode->first_attribute("a", 0, false);
+ if (attr)
+ dh.setAInMM(uLength.toMillimeter(convertToFloat(attr->value())));
+ attr = dhXMLNode->first_attribute("d", 0, false);
+ if (attr)
+ dh.setDInMM(uLength.toMillimeter(convertToFloat(attr->value())));
+ attr = dhXMLNode->first_attribute("alpha", 0, false);
+ if (attr)
+ dh.setAlphaRadian(convertToFloat(attr->value()), isRadian);
+ attr = dhXMLNode->first_attribute("theta", 0, false);
+ if (attr)
+ dh.setThetaRadian(convertToFloat(attr->value()), isRadian);
}
bool BaseIO::hasUnitsAttribute(rapidxml::xml_node<char> *node)
@@ -880,23 +925,15 @@ void BaseIO::processPhysicsTag(rapidxml::xml_node<char> *physicsXMLNode, const s
physics.intertiaMatrix *= factor;
}
- /*rapidxml::xml_node<> *velXMLNode = physicsXMLNode->first_node("maxVelocity",0,false);
- if (velXMLNode)
- {
- physics.maxVelocity = getFloatByAttributeName(velXMLNode,"value");
+ rapidxml::xml_node<> *ignoreColXMLNode = physicsXMLNode->first_node("ignorecollision",0,false);
+ while (ignoreColXMLNode)
+ {
+ rapidxml::xml_attribute<> *attr = ignoreColXMLNode->first_attribute("name", 0, false);
+ THROW_VR_EXCEPTION_IF(!attr, "Expecting 'name' attribute in <IgnoreCollision> tag..." << endl)
+ std::string s(attr->value());
+ physics.ignoreCollisions.push_back(s);
+ ignoreColXMLNode = ignoreColXMLNode->next_sibling("ignorecollision",0,false);
}
- rapidxml::xml_node<> *accXMLNode = physicsXMLNode->first_node("maxAcceleration",0,false);
- if (accXMLNode)
- {
- physics.maxAcceleration = getFloatByAttributeName(accXMLNode,"value");
- }
-
- rapidxml::xml_node<> *torXMLNode = physicsXMLNode->first_node("maxTorque",0,false);
- if (torXMLNode)
- {
- physics.maxTorque = getFloatByAttributeName(torXMLNode,"value");
- } */
-
}
std::string BaseIO::processFileNode( rapidxml::xml_node<char> *fileNode, const std::string &basePath )
diff --git a/VirtualRobot/XML/BaseIO.h b/VirtualRobot/XML/BaseIO.h
index 4764ca2e1..09fbe95eb 100644
--- a/VirtualRobot/XML/BaseIO.h
+++ b/VirtualRobot/XML/BaseIO.h
@@ -101,7 +101,8 @@ protected:
static bool hasUnitsAttribute(rapidxml::xml_node<char> *node);
static std::vector< Units > getUnitsAttributes(rapidxml::xml_node<char> *node);
static void getAllAttributes(rapidxml::xml_node<char> *node, const std::string &attrString, std::vector<std::string> &storeValues);
- static boost::mutex mutex;
+ static void processDHNode(rapidxml::xml_node<char> *dhXMLNode, DHParameter &dh);
+ static boost::mutex mutex;
};
}
diff --git a/VirtualRobot/XML/RobotIO.cpp b/VirtualRobot/XML/RobotIO.cpp
index a00cf22ed..16a7409bc 100644
--- a/VirtualRobot/XML/RobotIO.cpp
+++ b/VirtualRobot/XML/RobotIO.cpp
@@ -140,7 +140,9 @@ RobotNodePtr RobotIO::processJointNode(rapidxml::xml_node<char> *jointXMLNode, c
RobotPtr robot,
VisualizationNodePtr visualizationNode,
CollisionModelPtr collisionModel,
- SceneObject::Physics &physics
+ SceneObject::Physics &physics,
+ RobotNode::RobotNodeType rntype,
+ Eigen::Matrix4f &transformationMatrix
)
{
float jointLimitLow = (float)-M_PI;
@@ -161,9 +163,10 @@ RobotNodePtr RobotIO::processJointNode(rapidxml::xml_node<char> *jointXMLNode, c
if (!jointXMLNode)
{
// no <Joint> tag -> fixed joint
+ postJointTransform = transformationMatrix; // used by TransformationNodes
RobotNodeFactoryPtr fixedNodeFactory = RobotNodeFactory::fromName(RobotNodeFixedFactory::getName(), NULL);
if (fixedNodeFactory)
- robotNode = fixedNodeFactory->createRobotNode(robot, robotNodeName, visualizationNode, collisionModel, jointLimitLow, jointLimitHigh, jointOffset, preJointTransform, axis, postJointTransform, translationDir, physics);
+ robotNode = fixedNodeFactory->createRobotNode(robot, robotNodeName, visualizationNode, collisionModel, jointLimitLow, jointLimitHigh, jointOffset, preJointTransform, axis, postJointTransform, translationDir, physics, rntype);
return robotNode;
}
@@ -302,33 +305,8 @@ RobotNodePtr RobotIO::processJointNode(rapidxml::xml_node<char> *jointXMLNode, c
{
// check for wrongly defined nodes
THROW_VR_EXCEPTION_IF((prejointTransformNode || tmpXMLNodeAxis || tmpXMLNodeTranslation || postjointTransformNode), "DH specification can not be used together with Axis, TranslationDirection, PreJointTransform or PostJointTransform definitions in <Joint> tag of node " << robotNodeName << endl);
-
- std::vector< Units > unitsAttr = getUnitsAttributes(dhXMLNode);
- Units uAngle("rad");
- Units uLength("mm");
- for (size_t i=0;i<unitsAttr.size();i++)
- {
- if (unitsAttr[i].isAngle())
- uAngle = unitsAttr[i];
- if (unitsAttr[i].isLength())
- uLength = unitsAttr[i];
- }
-
- dh.isSet = true;
- bool isRadian = uAngle.isRadian();
-
- attr = dhXMLNode->first_attribute("a", 0, false);
- if (attr)
- dh.setAInMM(uLength.toMillimeter(convertToFloat(attr->value())));
- attr = dhXMLNode->first_attribute("d", 0, false);
- if (attr)
- dh.setDInMM(uLength.toMillimeter(convertToFloat(attr->value())));
- attr = dhXMLNode->first_attribute("alpha", 0, false);
- if (attr)
- dh.setAlphaRadian(convertToFloat(attr->value()), isRadian);
- attr = dhXMLNode->first_attribute("theta", 0, false);
- if (attr)
- dh.setThetaRadian(convertToFloat(attr->value()), isRadian);
+ processDHNode(dhXMLNode,dh);
+
} else
{
dh.isSet = false;
@@ -365,11 +343,11 @@ RobotNodePtr RobotIO::processJointNode(rapidxml::xml_node<char> *jointXMLNode, c
{
if (dh.isSet)
{
- robotNode = robotNodeFactory->createRobotNodeDH(robot, robotNodeName, visualizationNode, collisionModel, jointLimitLow, jointLimitHigh, jointOffset, dh, physics);
+ robotNode = robotNodeFactory->createRobotNodeDH(robot, robotNodeName, visualizationNode, collisionModel, jointLimitLow, jointLimitHigh, jointOffset, dh, physics, rntype);
} else
{
// create nodes that are not defined via DH parameters
- robotNode = robotNodeFactory->createRobotNode(robot, robotNodeName, visualizationNode, collisionModel, jointLimitLow, jointLimitHigh, jointOffset, preJointTransform, axis, postJointTransform, translationDir, physics);
+ robotNode = robotNodeFactory->createRobotNode(robot, robotNodeName, visualizationNode, collisionModel, jointLimitLow, jointLimitHigh, jointOffset, preJointTransform, axis, postJointTransform, translationDir, physics, rntype);
}
}
else
@@ -382,13 +360,15 @@ RobotNodePtr RobotIO::processJointNode(rapidxml::xml_node<char> *jointXMLNode, c
}
+
RobotNodePtr RobotIO::processRobotNode(rapidxml::xml_node<char> *robotNodeXMLNode,
RobotPtr robo,
const std::string &basePath,
int &robotNodeCounter,
std::vector< std::string > &childrenNames,
std::vector< ChildFromRobotDef > &childrenFromRobot,
- RobotDescription loadMode)
+ RobotDescription loadMode,
+ RobotNode::RobotNodeType rntype)
{
childrenFromRobot.clear();
THROW_VR_EXCEPTION_IF(!robotNodeXMLNode, "NULL data in processRobotNode");
@@ -418,6 +398,7 @@ RobotNodePtr RobotIO::processRobotNode(rapidxml::xml_node<char> *robotNodeXMLNod
RobotNodePtr robotNode;
SceneObject::Physics physics;
bool physicsDefined = false;
+ Eigen::Matrix4f transformMatrix = Eigen::Matrix4f::Identity();
rapidxml::xml_node<>* node = robotNodeXMLNode->first_node();
rapidxml::xml_node<>* jointNodeXML = NULL;
@@ -479,7 +460,10 @@ RobotNodePtr RobotIO::processRobotNode(rapidxml::xml_node<char> *robotNodeXMLNod
THROW_VR_EXCEPTION_IF(physicsDefined, "Two physics tags defined in RobotNode '" << robotNodeName << "'." << endl);
processPhysicsTag(node,robotNodeName,physics);
physicsDefined= true;
- } else
+ } else if (nodeName == "transform")
+ {
+ processTransformNode(robotNodeXMLNode,robotNodeName,transformMatrix);
+ } else
{
THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in RobotNode <" << robotNodeName << ">." << endl);
}
@@ -489,7 +473,7 @@ RobotNodePtr RobotIO::processRobotNode(rapidxml::xml_node<char> *robotNodeXMLNod
//create joint from xml data
- robotNode = processJointNode(jointNodeXML,robotNodeName, robo, visualizationNode, collisionModel, physics);
+ robotNode = processJointNode(jointNodeXML,robotNodeName, robo, visualizationNode, collisionModel, physics, rntype, transformMatrix);
return robotNode;
}
@@ -694,11 +678,19 @@ void RobotIO::processRobotChildNodes(rapidxml::xml_node<char>* robotXMLNode,
{
std::string nodeName_ = XMLNode->name();
std::string nodeName = getLowerCase(XMLNode->name());
- if (nodeName == "robotnode")
+ if (nodeName == "robotnode" || nodeName == "jointnode" || nodeName == "transformationnode" || nodeName == "bodynode")
{
std::vector< ChildFromRobotDef > childrenFromRobot;
std::vector< std::string > childrenNames;
- RobotNodePtr n = processRobotNode(XMLNode, robo, basePath, robotNodeCounter, childrenNames, childrenFromRobot,loadMode);
+ // check for type
+ RobotNode::RobotNodeType rntype = RobotNode::Generic;
+ if (nodeName == "jointnode")
+ rntype = RobotNode::Joint;
+ if (nodeName == "bodynode")
+ rntype = RobotNode::Body;
+ if (nodeName == "transformationnode")
+ rntype = RobotNode::Transform;
+ RobotNodePtr n = processRobotNode(XMLNode, robo, basePath, robotNodeCounter, childrenNames, childrenFromRobot,loadMode,rntype);
if (!n)
{
std::string failedNodeName = processNameAttribute(XMLNode);
diff --git a/VirtualRobot/XML/RobotIO.h b/VirtualRobot/XML/RobotIO.h
index 3347fc7b2..64f41401b 100644
--- a/VirtualRobot/XML/RobotIO.h
+++ b/VirtualRobot/XML/RobotIO.h
@@ -105,7 +105,8 @@ protected:
int &robotNodeCounter,
std::vector< std::string > &childrenNames,
std::vector< ChildFromRobotDef > &childrenFromRobot,
- RobotDescription loadMode = eFull);
+ RobotDescription loadMode = eFull,
+ RobotNode::RobotNodeType rntype = RobotNode::Generic);
static EndEffectorPtr processEndeffectorNode(rapidxml::xml_node<char>* endeffectorXMLNode, RobotPtr robo);
static EndEffectorActorPtr processEndeffectorActorNode(rapidxml::xml_node<char>* endeffectorActorXMLNode, RobotPtr robo);
static void processEndeffectorStaticNode(rapidxml::xml_node<char>* endeffectorStaticXMLNode, RobotPtr robo, std::vector<RobotNodePtr>& staticNodesList);
@@ -113,13 +114,13 @@ protected:
static void processActorNodeList(rapidxml::xml_node<char>* parentNode, RobotPtr robot, std::vector<EndEffectorActor::ActorDefinition>& actorList, bool clearList = true);
//static RobotNodeSetPtr processRobotNodeSet(rapidxml::xml_node<char> *setXMLNode, RobotPtr robo, const std::string &rootName, int &robotNodeSetCounter);
static void processChildNode(rapidxml::xml_node<char> *childXMLNode, std::vector<std::string> &childrenNames);
- static RobotNodePtr processJointNode(rapidxml::xml_node<char> *jointXMLNode, const std::string& robotNodeName,
- RobotPtr robot, VisualizationNodePtr visualizationNode, CollisionModelPtr collisionModel, SceneObject::Physics &physics);
+ static RobotNodePtr processJointNode( rapidxml::xml_node<char> *jointXMLNode, const std::string& robotNodeName,
+ RobotPtr robot, VisualizationNodePtr visualizationNode, CollisionModelPtr collisionModel,
+ SceneObject::Physics &physics, RobotNode::RobotNodeType rntype,Eigen::Matrix4f &transformationMatrix);
static void processChildFromRobotNode(rapidxml::xml_node<char> *childXMLNode, const std::string &nodeName, std::vector< ChildFromRobotDef > &childrenFromRobot);
static void processLimitsNode(rapidxml::xml_node<char> *limitsXMLNode, float &jointLimitLo, float &jointLimitHi);
static std::map<std::string,int> robot_name_counter;
static VisualizationNodePtr checkUseAsColModel(rapidxml::xml_node<char>* visuXMLNode, const std::string &robotNodeName, const std::string &basePath);
-
};
}
diff --git a/VirtualRobot/data/robots/ArmarIII/ArmarIII-Head.xml b/VirtualRobot/data/robots/ArmarIII/ArmarIII-Head.xml
index 551d9153a..9dd9dcff8 100644
--- a/VirtualRobot/data/robots/ArmarIII/ArmarIII-Head.xml
+++ b/VirtualRobot/data/robots/ArmarIII/ArmarIII-Head.xml
@@ -36,6 +36,7 @@
<Physics>
<CoM location="VisualizationBBoxCenter"/>
<Mass value="4" units="kg" />
+ <IgnoreCollision name="Hip Yaw"/>
</Physics>
<CollisionModel>
@@ -58,6 +59,8 @@
<Physics>
<CoM location="VisualizationBBoxCenter"/>
<Mass value="2" units="kg" />
+ <IgnoreCollision name="Hip Yaw"/>
+ <IgnoreCollision name="Head Base"/>
</Physics>
<CollisionModel>
@@ -98,6 +101,8 @@
<Physics>
<CoM location="VisualizationBBoxCenter"/>
<Mass value="6.79877000" units="kg" />
+ <IgnoreCollision name="Neck_2_Roll"/>
+ <IgnoreCollision name="Hip Yaw"/>
</Physics>
<Visualization>
diff --git a/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftArm.xml b/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftArm.xml
index 6180587cd..1de6bc1d9 100644
--- a/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftArm.xml
+++ b/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftArm.xml
@@ -53,6 +53,7 @@
<Physics>
<CoM location="VisualizationBBoxCenter"/>
<Mass value="1.55685000" units="kg" />
+ <IgnoreCollision name="Shoulder 1 L"/>
</Physics>
<Visualization enable="true">
<File type="Inventor">fullmodel/upperarm_l.iv</File>
@@ -129,6 +130,7 @@
<Physics>
<CoM location="VisualizationBBoxCenter"/>
<Mass value="2.59945309" units="kg" />
+ <IgnoreCollision name="Underarm L"/>
</Physics>
<Visualization enable="true">
<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
diff --git a/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightArm.xml b/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightArm.xml
index 67167acb2..4761bac35 100644
--- a/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightArm.xml
+++ b/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightArm.xml
@@ -52,6 +52,7 @@
<Physics>
<CoM location="VisualizationBBoxCenter"/>
<Mass value="1.55685000" units="kg" />
+ <IgnoreCollision name="Shoulder 1 R"/>
</Physics>
<Visualization enable="true">
<File type="Inventor">fullmodel/upperarm_r.iv</File>
@@ -134,6 +135,7 @@
<Physics>
<CoM location="VisualizationBBoxCenter"/>
<Mass value="2.59945309" units="kg" />
+ <IgnoreCollision name="Underarm R"/>
</Physics>
<ChildFromRobot>
<File importEEF="true">ArmarIII-RightHand.xml</File>
diff --git a/VirtualRobot/data/robots/ArmarIII/ArmarIII.xml b/VirtualRobot/data/robots/ArmarIII/ArmarIII.xml
index 49a5f4372..24d2e4b1b 100644
--- a/VirtualRobot/data/robots/ArmarIII/ArmarIII.xml
+++ b/VirtualRobot/data/robots/ArmarIII/ArmarIII.xml
@@ -18,9 +18,9 @@
</RobotNode>
<RobotNode name="X_Platform">
- <Joint type="prismatic">
+ <Joint type="fixed">
<DH a="0" d="0" theta="90" alpha="-90" units="degree"/>
- <Limits unit="mm" lo="-10000" hi="10000"/>
+ <!--Limits unit="mm" lo="-10000" hi="10000"/-->
</Joint>
<Visualization enable="true">
<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
@@ -29,9 +29,9 @@
</RobotNode>
<RobotNode name="Y_Platform">
- <Joint type="prismatic">
+ <Joint type="fixed">
<DH a="0" d="0" theta="-90" alpha="90" units="degree"/>
- <Limits unit="mm" lo="-10000" hi="10000"/>
+ <!--Limits unit="mm" lo="-10000" hi="10000"/-->
</Joint>
<Visualization enable="true">
<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
@@ -102,6 +102,7 @@
</Visualization>
<Physics>
<Mass value="10" units="kg" />
+ <IgnoreCollision name="Platform"/>
</Physics>
<CollisionModel>
<File type="Inventor">convexModel/platform_roll_link.iv</File>
@@ -117,6 +118,8 @@
<Physics>
<CoM location="VisualizationBBoxCenter"/>
<Mass value="15.415" units="kg" />
+ <IgnoreCollision name="Hip Pitch"/>
+ <IgnoreCollision name="Platform"/>
</Physics>
<Visualization enable="true">
<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
diff --git a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
index 0403aeab3..26be1ecf8 100644
--- a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
+++ b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
@@ -402,6 +402,11 @@ void showRobotWindow::loadRobot()
{
robotSep->removeAllChildren();
cout << "Loading Robot from " << m_sRobotFilename << endl;
+ currentEEF.reset();
+ currentRobotNode.reset();
+ currentRobotNodes.clear();
+ currentRobotNodeSet.reset();
+ robot.reset();
try
{
@@ -475,6 +480,38 @@ void showRobotWindow::closeHand()
void showRobotWindow::openHand()
{
+#if 1
+ if (robot)
+ {
+ float randMult = (float)(1.0/(double)(RAND_MAX));
+ std::vector<RobotNodePtr> rn = robot->getRobotNodes();
+ std::vector<RobotNodePtr> rnJoints;
+ for (size_t j=0;j<rn.size();j++)
+ {
+ if (rn[j]->isRotationalJoint())
+ rnJoints.push_back(rn[j]);
+ }
+ int loops = 10000;
+ clock_t startT = clock();
+ for (int i=0;i<loops;i++)
+ {
+ std::vector<float> jv;
+ for (size_t j=0;j<rnJoints.size();j++)
+ {
+ float t = (float)rand() * randMult; // value from 0 to 1
+ t = rnJoints[j]->getJointLimitLo() + (rnJoints[j]->getJointLimitHi() - rnJoints[j]->getJointLimitLo())*t;
+ jv.push_back(t);
+ }
+ robot->setJointValues(rnJoints,jv);
+ }
+ clock_t endT = clock();
+
+ float diffClock = (float)(((float)(endT - startT) / (float)CLOCKS_PER_SEC) * 1000.0f);
+ cout << "RobotNodes:" << rn.size() << endl;
+ cout << "Joints:" << rnJoints.size() << endl;
+ cout << "loops:" << loops << ". time (ms):" << diffClock << ". Per loop:" << diffClock/(float)loops << endl;
+ }
+#endif
if (currentEEF)
currentEEF->openActors();
}
--
GitLab