Skip to content
Snippets Groups Projects
Commit 81b2040f authored by Adrian Knobloch's avatar Adrian Knobloch
Browse files

Add translational joint to simulation and remove the 6DOF joint implementation 

The 6DOF joint implementation was not functional and not in use.
parent a68d36a3
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
+ 229
− 295
View file @ 81b2040f
......@@ -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
if (joint->isRotationalJoint())
Eigen::Vector3f directionLocal = rnPrisJoint->getJointTranslationDirectionJointCoordSystem();
jointbt = createTranslationalJoint(btBody1, btBody2, coordSystemNode1, coordSystemNode2, anchor_inNode1, anchor_inNode2, directionLocal, coordSystemJoint, limMin, limMax);
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,55 +530,104 @@ 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);
const ActuationMode& actuation = it->second.actuation;
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);
if (actuation.mode == 0)
{
m->m_enableMotor = false;
continue;
}
if (!it->second.node->isRotationalJoint() && !it->second.node->isTranslationalJoint())
continue;
m->m_enableMotor = true;
LinkInfo link = getLink(it->second.node);
if (actuation.modes.position && actuation.modes.velocity)
{
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);
}
else if (actuation.modes.velocity)
{
m->m_targetVelocity = controller.update(0, velocityTarget, actuation, dt);
}
const ActuationMode& actuation = it->second.actuation;
// 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));
btScalar velActual = btScalar(getJointSpeed(it->first));
btScalar velocityTarget = btScalar(it->second.jointVelocityTarget);
controller.setName(it->first->getName());
if (actuation.mode == 0)
if (it->second.node->isTranslationalJoint())
{
posTarget *= 0.001;
posActual *= 0.001;
}
// CHECK FOR DISABLED MOTORS
if (actuation.mode == 0)
{
if (it->second.node->isRotationalJoint())
{
boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
hinge->enableMotor(false);
continue;
}
else if (it->second.node->isTranslationalJoint() && !ignoreTranslationalJoints)
{
boost::shared_ptr<btSliderConstraint> slider = boost::dynamic_pointer_cast<btSliderConstraint>(link.joint);
slider->setPoweredLinMotor(false);
continue;
}
}
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
double targetVelocity;
double targetVelocity = 0;
if (actuation.modes.position && actuation.modes.velocity)
{
// cout << "################### " << it->second.node->getName() << " error: " << (posTarget - posActual) << ", velTarget:" << velocityTarget << endl;
......@@ -566,31 +648,6 @@ namespace SimDynamics
// if (it->first->getName() == "Upperarm R")
// cout << "################### " << target.node->getName() << " velActual:" << velActual << ", velTarget " << velocityTarget << " targetVelocity " << targetVelocity << " pos target: " << target.jointValueTarget << " posActual: " << posActual << endl;
}
else if (actuation.modes.torque)
{
//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);
}
btScalar maxImpulse = bulletMaxMotorImulse;
// controller.setCurrentVelocity(velActual);
......@@ -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);
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);
if (!hinge2)
{
return 0.0f;
}
btRotationalLimitMotor* m;
boost::shared_ptr<btSliderConstraint> slider = boost::dynamic_pointer_cast<btSliderConstraint>(link.joint);
VR_ASSERT(slider);
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,42 @@ 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 deltaVel = (link.dynNode2->getLinearVelocity() - link.dynNode1->getLinearVelocity()) * 1000 / BulletObject::ScaleFactor;
return deltaVel.norm();
}
else
{
VR_WARNING << "NYI" << 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)
......@@ -1260,6 +1183,10 @@ namespace SimDynamics
enableForceTorqueFeedback(link, true);
result = getJointForceTorqueGlobal(link).tail(3);
}
else if (rn->isTranslationalJoint())
{
VR_WARNING << "NYI" << endl;
}
return result;
}
......@@ -1277,17 +1204,20 @@ namespace SimDynamics
LinkInfo link = getLink(rn);
if (!rn->isRotationalJoint())
if (rn->isRotationalJoint())
{
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 if (rn->isTranslationalJoint())
{
VR_WARNING << "NYI" << endl;
}
return 0.0;
}
Eigen::Vector3f BulletRobot::getJointForces(RobotNodePtr rn)
......@@ -1310,6 +1240,10 @@ namespace SimDynamics
enableForceTorqueFeedback(link, true);
result = getJointForceTorqueGlobal(link).head(3);
}
else if (rn->isTranslationalJoint())
{
VR_WARNING << "NYI" << endl;
}
return result;
}
......
SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
+ 1
− 0
View file @ 81b2040f
......@@ -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;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment