diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp index 7ab9fe38bf735707ec542716d46ec7c9fff71c4f..95116421e50bcf0b9e9d1fa5bf53ea4090ca2fa1 100644 --- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp +++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp @@ -468,6 +468,10 @@ namespace SimDynamics // start standard actuator actuateNode(joint, joint->getJointValue()); } + else + { + addJointFriction(joint); + } #endif } @@ -1020,6 +1024,34 @@ namespace SimDynamics } } + void BulletRobot::addJointFriction(VirtualRobot::RobotNodePtr node) + { + MutexLockPtr lock = getScopedLock(); + VR_ASSERT(node); + + if (node->isRotationalJoint() || (node->isTranslationalJoint() && !ignoreTranslationalJoints)) + { + if (!hasLink(node)) + { + VR_ERROR << "No link for node " << node->getName() << std::endl; + return; + } + + DynamicsRobot::addJointFriction(node); + } + else + { + if (node->isTranslationalJoint() && ignoreTranslationalJoints) + { + VR_WARNING << "Translational joints ignored. (ignoring node " << node->getName() << ")." << std::endl; + } + else + { + VR_ERROR << "Only Revolute and translational joints implemented so far (ignoring node " << node->getName() << ")." << std::endl; + } + } + } + void BulletRobot::actuateNodeVel(RobotNodePtr node, double jointVelocity) { MutexLockPtr lock = getScopedLock(); diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h index 59301963d252389758cee9f4183077ecc48010de..12c18bdb1891d6d89ca7b5b6e3aeb2ae7e7bce93 100644 --- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h +++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h @@ -198,6 +198,9 @@ namespace SimDynamics */ double getMaximumMotorImpulse() const; + void addJointFriction(VirtualRobot::RobotNodePtr node); + + protected: void buildBulletModels(bool enableJointMotors); diff --git a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp index c5b0136c5b1e5152bdf3e27b4b5c10e77cc58df8..fc2de84bae59f39cf77c6fa11da03b1b6add418b 100644 --- a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp +++ b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp @@ -109,6 +109,37 @@ namespace SimDynamics actuateNode(robot->getRobotNode(node), jointValue); } + void DynamicsRobot::addJointFriction(VirtualRobot::RobotNodePtr node) + { + MutexLockPtr lock = getScopedLock(); + VR_ASSERT(robot); + VR_ASSERT(node); + VR_ASSERT(robot->hasRobotNode(node)); + + //if (!hasDynamicsRobotNode(node)) + // createDynamicsNode(node); + + //DynamicsObjectPtr dnyRN = getDynamicsRobotNode(node); + + robotNodeActuationTarget target; + target.actuation.modes.torque = 1; + target.node = node; + target.jointTorqueTarget = 1; + //target.dynNode = dnyRN; + + actuationTargets[node] = target; + + if (actuationControllers.find(node) == actuationControllers.end()) + { + actuationControllers[node] = VelocityMotorController(node->getMaxVelocity(), node->getMaxAcceleration()); + actuationControllers[node].reset(PID_p, PID_i, PID_d); + } + else + { + actuationControllers[node].reset(); + } + } + void DynamicsRobot::actuateNode(VirtualRobot::RobotNodePtr node, double jointValue) { MutexLockPtr lock = getScopedLock(); diff --git a/SimDynamics/DynamicsEngine/DynamicsRobot.h b/SimDynamics/DynamicsEngine/DynamicsRobot.h index 870c8708966de738718015f3436b57d8b78919ea..265f4b447d2c9a00fab0a57aeed9ff493cc89eb9 100644 --- a/SimDynamics/DynamicsEngine/DynamicsRobot.h +++ b/SimDynamics/DynamicsEngine/DynamicsRobot.h @@ -79,6 +79,8 @@ namespace SimDynamics virtual double getNodeTarget(VirtualRobot::RobotNodePtr node); virtual void enableActuation(ActuationMode mode); virtual void disableActuation(); + + void addJointFriction(VirtualRobot::RobotNodePtr node); /*! Usually this method is called by the framework in every tick to perform joint actuation.