diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
index b73366f4811e82cfbb065ba2f8b66fe34ce2d64e..680bb732dcd1a5c81f63c851998dbbbc6167b939 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
@@ -281,10 +281,9 @@ namespace SimDynamics
{
MutexLockPtr lock = getScopedLock();
/* convert to local coord system, apply comoffset and convert back*/
- Eigen::Matrix4f poseLocal = sceneObject->getGlobalPose().inverse() * pose;
- poseLocal.block(0, 3, 3, 1) += com;
- Eigen::Matrix4f poseGlobal = sceneObject->getGlobalPose() * poseLocal;
- this->rigidBody->setWorldTransform(BulletEngine::getPoseBullet(poseGlobal));
+ Eigen::Matrix4f poseCom = pose;
+ poseCom.block(0, 3, 3, 1) += com;
+ this->rigidBody->setWorldTransform(BulletEngine::getPoseBullet(poseCom));
// notify motionState of non-robot nodes
if(!boost::dynamic_pointer_cast<VirtualRobot::RobotNode>(sceneObject))
@@ -296,7 +295,7 @@ namespace SimDynamics
void BulletObject::setPose(const Eigen::Matrix4f& pose)
{
MutexLockPtr lock = getScopedLock();
- DynamicsObject::setPose(pose);
+ //DynamicsObject::setPose(pose);
setPoseIntern(pose);
}
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
index 73bc8835297cb36df37d79c8587bf1d8e16399ea..50ffce4a2b1bc70b054f46492019f4f32a1bb3b8 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
@@ -13,15 +13,12 @@
#include <VirtualRobot/Nodes/ForceTorqueSensor.h>
#include <VirtualRobot/Nodes/ContactSensor.h>
-// either hinge or generic6DOF constraints can be used
-//#define USE_BULLET_GENERIC_6DOF_CONSTRAINT
//#define DEBUG_FIXED_OBJECTS
//#define DEBUG_SHOW_LINKS
using namespace VirtualRobot;
using namespace std;
-//#define PRINT_TEST_DEBUG_MESSAGES
namespace SimDynamics
{
@@ -31,7 +28,7 @@ namespace SimDynamics
// should be enough for up to 10ms/step
, bulletMaxMotorImulse(30 * BulletObject::ScaleFactor)
{
- ignoreTranslationalJoints = true;
+ ignoreTranslationalJoints = false;
buildBulletModels(enableJointMotors);
@@ -221,12 +218,6 @@ namespace SimDynamics
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
-
// we need to align coord system joint, so that z-axis is rotation axis
MathTools::Quaternion q1 = MathTools::getRotation(Eigen::Vector3f::UnitZ(), axisLocal);
@@ -260,6 +251,35 @@ namespace SimDynamics
return hinge;
}
+ boost::shared_ptr<btTypedConstraint> BulletRobot::createTranslationalJoint(boost::shared_ptr<btRigidBody> btBody1, boost::shared_ptr<btRigidBody> btBody2, Eigen::Matrix4f &coordSystemNode1, Eigen::Matrix4f &coordSystemNode2, Eigen::Matrix4f& anchor_inNode1, Eigen::Matrix4f& anchor_inNode2, Eigen::Vector3f &directionLocal, Eigen::Matrix4f& coordSystemJoint, double limMinBT, double limMaxBT)
+ {
+ // we need to align coord system joint, so that z-axis is rotation axis
+
+ btVector3 pivot1 = BulletEngine::getVecBullet(anchor_inNode1.block(0, 3, 3, 1));
+ btVector3 pivot2 = BulletEngine::getVecBullet(anchor_inNode2.block(0, 3, 3, 1));
+
+ MathTools::Quaternion q1 = MathTools::getRotation(Eigen::Vector3f::UnitX(), directionLocal); // dont ask me why this has to be the X axis...
+ Eigen::Matrix4f rotationzAlignment = MathTools::quat2eigen4f(q1);
+ Eigen::Matrix4f coordSystemJoint_zAligned = coordSystemJoint * rotationzAlignment;
+
+ // now we need to pivot points in local coord systems
+ btTransform tr1 = BulletEngine::getPoseBullet(coordSystemNode1.inverse() * coordSystemJoint_zAligned);
+ btTransform tr2 = BulletEngine::getPoseBullet(coordSystemNode2.inverse() * coordSystemJoint_zAligned);
+ tr1.getOrigin() = pivot1;
+ tr2.getOrigin() = pivot2;
+
+ boost::shared_ptr<btSliderConstraint> joint(new btSliderConstraint(*btBody1, *btBody2, tr1, tr2, true));
+
+ // disable agular part
+ joint->setLowerAngLimit(btScalar(0));
+ joint->setUpperAngLimit(btScalar(0));
+
+ joint->setLowerLinLimit(btScalar(limMinBT));
+ joint->setUpperLinLimit(btScalar(limMaxBT));
+
+ return joint;
+ }
+
boost::shared_ptr<btTypedConstraint> BulletRobot::createFixedJoint(boost::shared_ptr<btRigidBody> btBody1, boost::shared_ptr<btRigidBody> btBody2, Eigen::Matrix4f& anchor_inNode1, Eigen::Matrix4f& anchor_inNode2)
{
btTransform localA, localB;
@@ -344,9 +364,22 @@ namespace SimDynamics
double vr2bulletOffset = 0.0f;
- THROW_VR_EXCEPTION_IF((!ignoreTranslationalJoints && joint->isTranslationalJoint()), "Translational joints nyi...");
+ if (joint->isTranslationalJoint() && !ignoreTranslationalJoints)
+ {
+ boost::shared_ptr<RobotNodePrismatic> rnPrisJoint = boost::dynamic_pointer_cast<RobotNodePrismatic>(joint);
+
+ double limMin, limMax;
+ btScalar diff = joint->getJointValueOffset();
+ limMin = (joint->getJointLimitLo() + diff) / 1000 * BulletObject::ScaleFactor; //mm -> m
+ limMax = (joint->getJointLimitHi() + diff) / 1000 * BulletObject::ScaleFactor; //mm -> m
+
+ Eigen::Vector3f directionLocal = rnPrisJoint->getJointTranslationDirectionJointCoordSystem();
+
+ jointbt = createTranslationalJoint(btBody1, btBody2, coordSystemNode1, coordSystemNode2, anchor_inNode1, anchor_inNode2, directionLocal, coordSystemJoint, limMin, limMax);
- if (joint->isRotationalJoint())
+ vr2bulletOffset = diff;
+ }
+ else if (joint->isRotationalJoint())
{
// create joint
boost::shared_ptr<RobotNodeRevolute> rnRevJoint = boost::dynamic_pointer_cast<RobotNodeRevolute>(joint);
@@ -433,7 +466,7 @@ namespace SimDynamics
links.push_back(i);
#ifndef DEBUG_FIXED_OBJECTS
- if (enableJointMotors && joint->isRotationalJoint())
+ if (enableJointMotors && (joint->isRotationalJoint() || (joint->isTranslationalJoint()) && !ignoreTranslationalJoints))
{
// start standard actuator
actuateNode(joint, joint->getJointValue());
@@ -497,99 +530,123 @@ namespace SimDynamics
//cout << "it:" << it->first << ", name: " << it->first->getName() << endl;
VelocityMotorController& controller = actuationControllers[it->first];
- if (it->second.node->isRotationalJoint())
- {
- LinkInfo link = getLink(it->second.node);
+ if (!it->second.node->isRotationalJoint() && !it->second.node->isTranslationalJoint())
+ continue;
- const ActuationMode& actuation = it->second.actuation;
+ LinkInfo link = getLink(it->second.node);
- btScalar posTarget = btScalar(it->second.jointValueTarget + link.jointValueOffset);
- btScalar posActual = btScalar(getJointAngle(it->first));
- btScalar velActual = btScalar(getJointSpeed(it->first));
- btScalar velocityTarget = btScalar(it->second.jointVelocityTarget);
- controller.setName(it->first->getName());
-#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
- boost::shared_ptr<btGeneric6DofConstraint> dof = boost::dynamic_pointer_cast<btGeneric6DofConstraint>(link.joint);
- VR_ASSERT(dof);
- btRotationalLimitMotor* m = dof->getRotationalLimitMotor(0);
- VR_ASSERT(m);
+ const ActuationMode& actuation = it->second.actuation;
- if (actuation.mode == 0)
+ // CHECK FOR DISABLED MOTORS
+ if (actuation.mode == 0)
+ {
+ if (it->second.node->isRotationalJoint())
{
- m->m_enableMotor = false;
+ boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
+ hinge->enableMotor(false);
continue;
}
-
- m->m_enableMotor = true;
-
- if (actuation.modes.position && actuation.modes.velocity)
+ else if (it->second.node->isTranslationalJoint() && !ignoreTranslationalJoints)
{
- m->m_targetVelocity = controller.update(posTarget - posActual, velocityTarget, actuation, dt);
- }
- else if (actuation.modes.position)
- {
- m->m_targetVelocity = controller.update(posTarget - posActual, 0, actuation, dt);
+ boost::shared_ptr<btSliderConstraint> slider = boost::dynamic_pointer_cast<btSliderConstraint>(link.joint);
+ slider->setPoweredLinMotor(false);
+ continue;
}
- else if (actuation.modes.velocity)
+ }
+
+
+ if (actuation.modes.torque)
+ {
+ // TORQUE MODES
+
+ if (it->second.node->isRotationalJoint())
+ {
+ boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
+ auto torque = it->second.jointTorqueTarget;
+ btVector3 hingeAxisLocalA =
+ hinge->getFrameOffsetA().getBasis().getColumn(2);
+ btVector3 hingeAxisLocalB =
+ hinge->getFrameOffsetB().getBasis().getColumn(2);
+ btVector3 hingeAxisWorldA =
+ hinge->getRigidBodyA().getWorldTransform().getBasis() *
+ hingeAxisLocalA;
+ btVector3 hingeAxisWorldB =
+ hinge->getRigidBodyB().getWorldTransform().getBasis() *
+ hingeAxisLocalB;
+
+
+ int sign = torque > 0?1:-1;
+ torque = std::min<double>(fabs(torque), it->first->getMaxTorque()) * sign;
+
+ btVector3 hingeTorqueA = - torque * hingeAxisWorldA;
+ btVector3 hingeTorqueB = torque * hingeAxisWorldB;
+ hinge->enableMotor(false);
+ hinge->getRigidBodyA().applyTorque(hingeTorqueA);
+ hinge->getRigidBodyB().applyTorque(hingeTorqueB);
+ }
+ else
+ {
+ // not yet tested!
+ boost::shared_ptr<btSliderConstraint> slider = boost::dynamic_pointer_cast<btSliderConstraint>(link.joint);
+ auto torque = it->second.jointTorqueTarget;
+ btVector3 sliderAxisLocalA =
+ slider->getFrameOffsetA().getBasis().getColumn(2);
+ btVector3 sliderAxisLocalB =
+ slider->getFrameOffsetB().getBasis().getColumn(2);
+ btVector3 sliderAxisWorldA =
+ slider->getRigidBodyA().getWorldTransform().getBasis() *
+ sliderAxisLocalA;
+ btVector3 sliderAxisWorldB =
+ slider->getRigidBodyB().getWorldTransform().getBasis() *
+ sliderAxisLocalB;
+
+
+ int sign = torque > 0?1:-1;
+ torque = std::min<double>(fabs(torque), it->first->getMaxTorque()) * sign;
+
+ btVector3 sliderTorqueA = - torque * sliderAxisWorldA;
+ btVector3 sliderTorqueB = torque * sliderAxisWorldB;
+ slider->setPoweredLinMotor(false);
+ slider->btTypedConstraint::getRigidBodyA().applyCentralForce(sliderTorqueA);
+ slider->btTypedConstraint::getRigidBodyB().applyCentralForce(sliderTorqueB);
+ }
+ }
+ else if (actuation.modes.position || actuation.modes.velocity)
+ {
+ // POSITION, VELOCITY OR POSITION&VELOCITY MODE
+
+ btScalar velActual = btScalar(getJointSpeed(it->first));
+ btScalar velocityTarget = btScalar(it->second.jointVelocityTarget);
+
+ if (actuation.modes.velocity && !actuation.modes.position)
{
- m->m_targetVelocity = controller.update(0, velocityTarget, actuation, dt);
+ // bullet is buggy here and cannot reach velocity targets for some joints, use a position-velocity mode as workaround
+ it->second.jointValueTarget += velocityTarget * dt;
}
- // FIXME torque based control is ignored
-#else
- boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
+ btScalar posTarget = btScalar(it->second.jointValueTarget + link.jointValueOffset);
+ btScalar posActual = btScalar(getJointAngle(it->first));
+ controller.setName(it->first->getName());
- if (actuation.mode == 0)
+ if (it->second.node->isTranslationalJoint())
{
- hinge->enableMotor(false);
- continue;
+ posTarget *= 0.001;
+ posActual *= 0.001;
+ velActual *= 0.001;
+ velocityTarget *= 0.001;
}
double targetVelocity;
- if (actuation.modes.position && actuation.modes.velocity)
+ if (!actuation.modes.position)
{
-// cout << "################### " << it->second.node->getName() << " error: " << (posTarget - posActual) << ", velTarget:" << velocityTarget << endl;
- targetVelocity = controller.update(posTarget - posActual, velocityTarget, actuation, btScalar(dt));
- }
- else if (actuation.modes.position)
- {
-// cout << "################### " << it->second.node->getName() << " error: " << (posTarget - posActual) << ", posTarget " << posTarget << endl;
- targetVelocity = controller.update(posTarget - posActual, 0.0, actuation, btScalar(dt));
- }
- else if (actuation.modes.velocity)
- {
- // bullet is buggy here and cannot reach velocity targets for some joints, use a position-velocity mode as workaround
- it->second.jointValueTarget += velocityTarget * dt;
+ // we always use position or position-velocity mode
ActuationMode tempAct = actuation;
tempAct.modes.position = 1;
- targetVelocity = controller.update(it->second.jointValueTarget - posActual, velocityTarget, tempAct, btScalar(dt));
-// if (it->first->getName() == "Upperarm R")
-// cout << "################### " << target.node->getName() << " velActual:" << velActual << ", velTarget " << velocityTarget << " targetVelocity " << targetVelocity << " pos target: " << target.jointValueTarget << " posActual: " << posActual << endl;
+ targetVelocity = controller.update(posTarget - posActual, velocityTarget, tempAct, btScalar(dt));
}
- else if (actuation.modes.torque)
+ else
{
- //cout << "################### torque:" << it->second.jointTorqueTarget << endl;
- auto torque = it->second.jointTorqueTarget;
- btVector3 hingeAxisLocalA =
- hinge->getFrameOffsetA().getBasis().getColumn(2);
- btVector3 hingeAxisLocalB =
- hinge->getFrameOffsetB().getBasis().getColumn(2);
- btVector3 hingeAxisWorldA =
- hinge->getRigidBodyA().getWorldTransform().getBasis() *
- hingeAxisLocalA;
- btVector3 hingeAxisWorldB =
- hinge->getRigidBodyB().getWorldTransform().getBasis() *
- hingeAxisLocalB;
-
-
- int sign = torque > 0?1:-1;
- torque = std::min<double>(fabs(torque), it->first->getMaxTorque()) * sign;
-
- btVector3 hingeTorqueA = - torque * hingeAxisWorldA;
- btVector3 hingeTorqueB = torque * hingeAxisWorldB;
- hinge->enableMotor(false);
- hinge->getRigidBodyA().applyTorque(hingeTorqueA);
- hinge->getRigidBodyB().applyTorque(hingeTorqueB);
+ targetVelocity = controller.update(posTarget - posActual, velocityTarget, actuation, btScalar(dt));
}
btScalar maxImpulse = bulletMaxMotorImulse;
@@ -599,29 +656,23 @@ namespace SimDynamics
maxImpulse = it->second.node->getMaxTorque() * btScalar(dt) * BulletObject::ScaleFactor * BulletObject::ScaleFactor * BulletObject::ScaleFactor;
//cout << "node:" << it->second.node->getName() << ", max impulse: " << maxImpulse << ", dt:" << dt << ", maxImp:" << it->second.node->getMaxTorque() << endl;
}
-#ifdef PRINT_TEST_DEBUG_MESSAGES
-
- if (it->first->getName() == "LegL_Ank1_joint")
-// if(posTarget - posActual > 0.05)
- {
- double p,i,d;
- controller.getPosPID(p,i,d);
- std::cout << it->first->getName() << ": " << (actuation.modes.velocity?true:false) << " " << (actuation.modes.position?true:false) << " p: " << p;
- cout << "################### " << it->first->getName() <<" error: " << (posTarget - posActual)<< ", actvel:" << velActual << ", target vel:" << targetVelocity << ", maxImpulse: " << maxImpulse << " applied impulse " << hinge->internalGetAppliedImpulse() << endl;
- }
-
-#endif
if(fabs(targetVelocity) > 0.00001)
{
link.dynNode1->getRigidBody()->activate();
link.dynNode2->getRigidBody()->activate();
}
- if (!actuation.modes.torque)
+ if (it->second.node->isRotationalJoint())
{
+ boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
hinge->enableAngularMotor(true, btScalar(targetVelocity), maxImpulse);
}
-
-#endif
+ else if (it->second.node->isTranslationalJoint() && !ignoreTranslationalJoints)
+ {
+ boost::shared_ptr<btSliderConstraint> slider = boost::dynamic_pointer_cast<btSliderConstraint>(link.joint);
+ slider->setMaxLinMotorForce(maxImpulse * 1000); // Magic number!!!
+ slider->setTargetLinMotorVelocity(btScalar(targetVelocity));
+ slider->setPoweredLinMotor(true);
+ }
}
}
@@ -957,7 +1008,7 @@ namespace SimDynamics
MutexLockPtr lock = getScopedLock();
VR_ASSERT(node);
- if (node->isRotationalJoint())
+ if (node->isRotationalJoint() || (node->isTranslationalJoint() && !ignoreTranslationalJoints))
{
if (!hasLink(node))
{
@@ -965,52 +1016,18 @@ namespace SimDynamics
return;
}
- LinkInfo link = getLink(node);
-#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
- boost::shared_ptr<btGeneric6DofConstraint> dof = boost::dynamic_pointer_cast<btGeneric6DofConstraint>(link.joint);
- VR_ASSERT(dof);
- btRotationalLimitMotor* m = dof->getRotationalLimitMotor(0);
- VR_ASSERT(m);
- m->m_enableMotor = true;
- m->m_maxMotorForce = 5;//bulletMaxMotorImulse; //?!
- m->m_maxLimitForce = 300;
DynamicsRobot::actuateNode(node, jointValue);
-#else
- boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
-
- /*if (!hinge)
+ }
+ else
+ {
+ if (node->isTranslationalJoint() && ignoreTranslationalJoints)
{
- // 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;
+ VR_WARNING << "Translational joints ignored. (ignoring node " << node->getName() << ")." << endl;
}
else
{
- //hinge->enableAngularMotor(true,0.0f,bulletMaxMotorImulse);// is max impulse ok?! (10 seems to be ok, 1 oscillates)
- }*/
-
- DynamicsRobot::actuateNode(node, jointValue);
-#endif
- }
- else
- {
- VR_ERROR << "Only Revolute joints implemented so far (ignoring node " << node->getName() << ")." << endl;
+ VR_ERROR << "Only Revolute and translational joints implemented so far (ignoring node " << node->getName() << ")." << endl;
+ }
}
}
@@ -1018,60 +1035,22 @@ namespace SimDynamics
{
MutexLockPtr lock = getScopedLock();
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);
- VR_ASSERT(dof);
- btRotationalLimitMotor* m = dof->getRotationalLimitMotor(0);
- VR_ASSERT(m);
- m->m_enableMotor = true;
- m->m_maxMotorForce = 5;//bulletMaxMotorImulse; //?!
- m->m_maxLimitForce = 300;
- DynamicsRobot::actuateNodeVel(node, jointVelocity);
-#else
- boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
- /*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
- }
+ if (!hasLink(node))
+ {
+ VR_ERROR << "No link for node " << node->getName() << endl;
+ return;
+ }
- VR_ASSERT(m);
- m->m_enableMotor = true;
- m->m_maxMotorForce = 5;//bulletMaxMotorImulse; //?!
- m->m_maxLimitForce = 300;
- }
- else
- {
- //hinge->enableAngularMotor(true,jointVelocity,bulletMaxMotorImulse);// is max impulse ok?! (10 seems to be ok, 1 oscillates)
- }*/
+ LinkInfo link = getLink(node);
+ if (node->isRotationalJoint() || node->isTranslationalJoint())
+ {
DynamicsRobot::actuateNodeVel(node, jointVelocity); // inverted joint direction in bullet
-#endif
}
else
{
- VR_ERROR << "Only Revolute joints implemented so far (node: " << node->getName() <<")..." << endl;
+ VR_ERROR << "Only Revolute and Prismatic joints implemented so far (node: " << node->getName() <<")..." << endl;
}
}
@@ -1087,59 +1066,25 @@ namespace SimDynamics
}
LinkInfo link = getLink(rn);
-#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
- boost::shared_ptr<btGeneric6DofConstraint> dof = boost::dynamic_pointer_cast<btGeneric6DofConstraint>(link.joint);
- VR_ASSERT(dof);
- btRotationalLimitMotor* m = dof->getRotationalLimitMotor(0);
- VR_ASSERT(m);
- dof->calculateTransforms();
- double a1 = dof->getAngle(0);
- double a2 = m->m_currentPosition;
- if (fabs(a1 - a2) > 0.05f)
+ if (rn->isRotationalJoint())
{
- VR_INFO << "Angle diff " << a1 << ", " << a2 << endl;
- }
-
- return (a2 - link.jointValueOffset); // inverted joint direction in bullet
-#else
- boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
- if (!hinge)
- return 0.0f;
+ boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
+ VR_ASSERT(hinge);
- /*if (!hinge)
+ return (hinge->getHingeAngle() - link.jointValueOffset); // inverted joint direction in bullet
+ }
+ else if (rn->isTranslationalJoint())
{
- // hinge2 / universal joint
- boost::shared_ptr<btUniversalConstraint> hinge2 = boost::dynamic_pointer_cast<btUniversalConstraint>(link.joint);
+ boost::shared_ptr<btSliderConstraint> slider = boost::dynamic_pointer_cast<btSliderConstraint>(link.joint);
+ VR_ASSERT(slider);
- 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();
- double a2 = m->m_currentPosition;
- return (a2 - link.jointValueOffset); // inverted joint direction in bullet
- }*/
-
- return (hinge->getHingeAngle() - link.jointValueOffset); // inverted joint direction in bullet
-#endif
+ return (slider->getLinearPos() - link.jointValueOffset) * 1000 / BulletObject::ScaleFactor; // m -> mm
+ }
+ else
+ {
+ return 0.0;
+ }
}
double BulletRobot::getJointTargetSpeed(VirtualRobot::RobotNodePtr rn)
@@ -1154,38 +1099,19 @@ namespace SimDynamics
}
LinkInfo link = getLink(rn);
- boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
- /*if (!hinge)
+ if (rn->isRotationalJoint())
{
- // 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;
- }
+ boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
- VR_ASSERT(m);
- return m->m_targetVelocity;
- }*/
+ return hinge->getMotorTargetVelosity();
+ }
+ else if (rn->isTranslationalJoint())
+ {
+ boost::shared_ptr<btSliderConstraint> slider = boost::dynamic_pointer_cast<btSliderConstraint>(link.joint);
- return hinge->getMotorTargetVelosity();
+ return slider->getTargetLinMotorVelocity() * 1000; // / BulletObject::ScaleFactor; m -> mm
+ }
}
double BulletRobot::getJointSpeed(VirtualRobot::RobotNodePtr rn)
@@ -1199,45 +1125,43 @@ namespace SimDynamics
return 0.0f;
}
- LinkInfo link = getLink(rn);
-#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
- VR_WARNING << "NYI" << endl;
- return 0.0;
-#else
- boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
-
- if (!hinge)
+ if (rn->isRotationalJoint())
{
- VR_WARNING << "NYI" << endl;
- return 0.0;
- }
+ LinkInfo link = getLink(rn);
+ boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
- boost::shared_ptr<RobotNodeRevolute> rnRevJoint = boost::dynamic_pointer_cast<RobotNodeRevolute>(link.nodeJoint);
+ VR_ASSERT(hinge);
- Eigen::Vector3f deltaVel = link.dynNode2->getAngularVelocity() - link.dynNode1->getAngularVelocity();
- double speed = deltaVel.dot(rnRevJoint->getJointRotationAxis());
- return speed;//hinge->getMotorTargetVelosity();
+ boost::shared_ptr<RobotNodeRevolute> rnRevJoint = boost::dynamic_pointer_cast<RobotNodeRevolute>(link.nodeJoint);
- /*
- * // does the same:
- double result = 0;
- Eigen::Vector3f globalAxis = rnRevJoint->getJointRotationAxis();
- result += globalAxis.dot(link.dynNode2->getAngularVelocity());
- result -= globalAxis.dot(link.dynNode1->getAngularVelocity());
- return result;*/
+ Eigen::Vector3f deltaVel = link.dynNode2->getAngularVelocity() - link.dynNode1->getAngularVelocity();
+ double speed = deltaVel.dot(rnRevJoint->getJointRotationAxis());
+ return speed;//hinge->getMotorTargetVelosity();
+ }
+ else if (rn->isTranslationalJoint())
+ {
+ LinkInfo link = getLink(rn);
+ boost::shared_ptr<btSliderConstraint> slider = boost::dynamic_pointer_cast<btSliderConstraint>(link.joint);
-#endif
+ VR_ASSERT(slider);
+
+ boost::shared_ptr<RobotNodePrismatic> rnPrisJoint = boost::dynamic_pointer_cast<RobotNodePrismatic>(link.nodeJoint);
+
+ Eigen::Vector3f jointDirection = rnPrisJoint->getGlobalPose().block<3, 3>(0, 0) * rnPrisJoint->getJointTranslationDirectionJointCoordSystem();
+ Eigen::Vector3f deltaVel = (link.dynNode2->getLinearVelocity() - link.dynNode1->getLinearVelocity());
+ return jointDirection.dot(deltaVel) / jointDirection.norm();
+ }
+ else
+ {
+ VR_WARNING << "Only translational and rotational joints implemented." << endl;
+ return 0.0;
+ }
}
double BulletRobot::getNodeTarget(VirtualRobot::RobotNodePtr node)
{
MutexLockPtr lock = getScopedLock();
-#ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
- return DynamicsRobot::getNodeTarget(node);
-#else
return DynamicsRobot::getNodeTarget(node);
-#endif
-
}
Eigen::Vector3f BulletRobot::getJointTorques(RobotNodePtr rn)
@@ -1255,11 +1179,15 @@ namespace SimDynamics
LinkInfo link = getLink(rn);
- if (rn->isRotationalJoint())
+ if (rn->isRotationalJoint() || rn->isTranslationalJoint())
{
enableForceTorqueFeedback(link, true);
result = getJointForceTorqueGlobal(link).tail(3);
}
+ else
+ {
+ VR_WARNING << "Only translational and rotational joints implemented." << endl;
+ }
return result;
}
@@ -1277,17 +1205,20 @@ namespace SimDynamics
LinkInfo link = getLink(rn);
- if (!rn->isRotationalJoint())
+ if (rn->isRotationalJoint() || rn->isTranslationalJoint())
{
- return 0.0;
- }
-
- enableForceTorqueFeedback(link, true);
- Eigen::Vector3f torqueVector = getJointForceTorqueGlobal(link).tail(3);
+ enableForceTorqueFeedback(link, true);
+ Eigen::Vector3f torqueVector = getJointForceTorqueGlobal(link).tail(3);
- // project onto joint axis
- double troque = (torqueVector.adjoint() * link.nodeJoint->getGlobalPose().block(0, 2, 3, 1))(0, 0);
- return troque;
+ // project onto joint axis
+ double troque = (torqueVector.adjoint() * link.nodeJoint->getGlobalPose().block(0, 2, 3, 1))(0, 0);
+ return troque;
+ }
+ else
+ {
+ VR_WARNING << "Only translational and rotational joints implemented." << endl;
+ }
+ return 0.0;
}
Eigen::Vector3f BulletRobot::getJointForces(RobotNodePtr rn)
@@ -1305,11 +1236,15 @@ namespace SimDynamics
LinkInfo link = getLink(rn);
- if (rn->isRotationalJoint())
+ if (rn->isRotationalJoint() || rn->isTranslationalJoint())
{
enableForceTorqueFeedback(link, true);
result = getJointForceTorqueGlobal(link).head(3);
}
+ else
+ {
+ VR_WARNING << "Only translational and rotational joints implemented." << endl;
+ }
return result;
}
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
index e94a9599da76f2c806f574a8c467a669c9673bf7..d8d1395f2a504f8dcf222375e62f9f5085d62a9b 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
@@ -232,6 +232,7 @@ namespace SimDynamics
btScalar bulletMaxMotorImulse;
boost::shared_ptr<btTypedConstraint> createFixedJoint(boost::shared_ptr<btRigidBody> btBody1, boost::shared_ptr<btRigidBody> btBody2, Eigen::Matrix4f& anchor_inNode1, Eigen::Matrix4f& anchor_inNode2);
boost::shared_ptr<btTypedConstraint> createHingeJoint(boost::shared_ptr<btRigidBody> btBody1, boost::shared_ptr<btRigidBody> btBody2, Eigen::Matrix4f& coordSystemNode1, Eigen::Matrix4f& coordSystemNode2, Eigen::Matrix4f& anchor_inNode1, Eigen::Matrix4f& anchor_inNode2, Eigen::Vector3f& axisGlobal, Eigen::Vector3f& axisLocal, Eigen::Matrix4f& coordSystemJoint, double limMinBT, double limMaxBT);
+ boost::shared_ptr<btTypedConstraint> createTranslationalJoint(boost::shared_ptr<btRigidBody> btBody1, boost::shared_ptr<btRigidBody> btBody2, Eigen::Matrix4f& coordSystemNode1, Eigen::Matrix4f& coordSystemNode2, Eigen::Matrix4f& anchor_inNode1, Eigen::Matrix4f& anchor_inNode2, Eigen::Vector3f& directionLocal, Eigen::Matrix4f& coordSystemJoint, double limMinBT, double limMaxBT);
bool ignoreTranslationalJoints;
diff --git a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
index b9d252f78c4aff2e95a57418fcc24c232f75bb19..1558a3b90cb189e3174468e7d9e280557b3bd439 100644
--- a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
+++ b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
@@ -396,15 +396,13 @@ namespace SimDynamics
void DynamicsRobot::setGlobalPose(const Eigen::Matrix4f& gp)
{
MutexLockPtr lock = getScopedLock();
- Eigen::Matrix4f currentPose = robot->getGlobalPose();
- Eigen::Matrix4f delta = gp * currentPose.inverse();
- robot->setGlobalPose(gp);
std::map<VirtualRobot::RobotNodePtr, DynamicsObjectPtr>::iterator it = dynamicRobotNodes.begin();
+ robot->setGlobalPose(gp);
while (it != dynamicRobotNodes.end())
{
- Eigen::Matrix4f newPose = it->second->getSceneObject()->getGlobalPose() * delta;
+ Eigen::Matrix4f newPose = it->second->getSceneObject()->getGlobalPose();
it->second->setPose(newPose);
it++;
}
diff --git a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
index da321445cda255d92f6718fbb9713e249c063a98..c52db5f787da3bc4a1a5b37b70215012f04aa69d 100644
--- a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
+++ b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
@@ -307,16 +307,11 @@ void SimDynamicsWindow::updateJoints()
for (size_t i = 0; i < nodes.size(); i++)
{
- if (nodes[i]->isRotationalJoint())
+ if (nodes[i]->isRotationalJoint() || nodes[i]->isTranslationalJoint())
{
- RobotNodeRevolutePtr rn = boost::dynamic_pointer_cast<RobotNodeRevolute>(nodes[i]);
-
- if (rn)
- {
- robotNodes.push_back(rn);
- QString qstr(rn->getName().c_str());
- UI.comboBoxRobotNode->addItem(qstr);
- }
+ robotNodes.push_back(nodes[i]);
+ QString qstr(nodes[i]->getName().c_str());
+ UI.comboBoxRobotNode->addItem(qstr);
}
}
@@ -382,7 +377,7 @@ bool SimDynamicsWindow::loadRobot(std::string robotFilename)
void SimDynamicsWindow::selectRobotNode(int n)
{
UI.comboBoxRobotNode->setCurrentIndex(n);
- RobotNodeRevolutePtr rn;
+ RobotNodePtr rn;
if (n >= 0 && n < (int)robotNodes.size())
{
@@ -428,12 +423,16 @@ void SimDynamicsWindow::updateJointInfo()
QString qVisu("VISU (simox): 0/0/0");
QString qCom("COM (bullet): 0/0/0");
QString tmp;
- RobotNodeRevolutePtr rn;
+ RobotNodePtr rn;
if (n >= 0 && n < (int)robotNodes.size())
{
rn = robotNodes[n];
}
+ else
+ {
+ return;
+ }
SimDynamics::DynamicsObjectPtr dynRN = dynamicsRobot->getDynamicsRobotNode(rn);
SimDynamics::BulletObjectPtr bulletRN = boost::dynamic_pointer_cast<SimDynamics::BulletObject>(dynRN);
@@ -772,7 +771,7 @@ void SimDynamicsWindow::jointValueChanged(int n)
#endif
*/
int j = UI.comboBoxRobotNode->currentIndex();
- RobotNodeRevolutePtr rn;
+ RobotNodePtr rn;
if (j >= 0 && j < (int)robotNodes.size())
{
diff --git a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h
index 0fe095ef7d94caa537cb98ad521c1ed4f41e44a0..adbe11a5a2f80561f8cd7943741b525829e50e64 100644
--- a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h
+++ b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h
@@ -104,7 +104,7 @@ protected:
SoTimerSensor* timerSensor;
- std::vector<VirtualRobot::RobotNodeRevolutePtr> robotNodes;
+ std::vector<VirtualRobot::RobotNodePtr> robotNodes;
std::map< VirtualRobot::RobotNodePtr, SoSeparator* > comVisuMap;