From ad570ead987080f68290a037607bb3308dda4ef5 Mon Sep 17 00:00:00 2001
From: themarex <themarex@042f3d55-54a8-47e9-b7fb-15903f145c44>
Date: Tue, 3 Jun 2014 09:52:36 +0000
Subject: [PATCH] Control motors in robot by PID controller
git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@599 042f3d55-54a8-47e9-b7fb-15903f145c44
---
SimDynamics/CMakeLists.txt | 2 +
.../BulletEngine/BulletRobot.cpp | 326 +++++++++---------
SimDynamics/DynamicsEngine/DynamicsRobot.cpp | 59 +++-
SimDynamics/DynamicsEngine/DynamicsRobot.h | 24 +-
SimDynamics/DynamicsEngine/DynamicsUtils.cpp | 133 +++++++
SimDynamics/DynamicsEngine/DynamicsUtils.h | 113 ++++++
.../SimDynamicsViewer/simDynamicsWindow.cpp | 8 +-
7 files changed, 466 insertions(+), 199 deletions(-)
create mode 100644 SimDynamics/DynamicsEngine/DynamicsUtils.cpp
create mode 100644 SimDynamics/DynamicsEngine/DynamicsUtils.h
diff --git a/SimDynamics/CMakeLists.txt b/SimDynamics/CMakeLists.txt
index 073b01b20..e0750d9e8 100644
--- a/SimDynamics/CMakeLists.txt
+++ b/SimDynamics/CMakeLists.txt
@@ -133,6 +133,7 @@ if (SimDynamics_DYNAMICSENGINE)
DynamicsEngine/DynamicsObject.cpp
DynamicsEngine/DynamicsEngine.cpp
DynamicsEngine/DynamicsRobot.cpp
+ DynamicsEngine/DynamicsUtils.cpp
)
SET(INCLUDES
@@ -142,6 +143,7 @@ if (SimDynamics_DYNAMICSENGINE)
DynamicsEngine/DynamicsObject.h
DynamicsEngine/DynamicsEngine.h
DynamicsEngine/DynamicsRobot.h
+ DynamicsEngine/DynamicsUtils.h
)
# ${SimDynamics_SimoxDir}/VirtualRobot/definesVR.h
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
index d8f929cc2..10a1a7040 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
@@ -899,214 +899,204 @@ void BulletRobot::actuateJoints(btScalar dt)
int jointCounter = 0;
- while (it!=actuationTargets.end())
- {
- //BulletObjectPtr drn;
- //if (it->second.dynNode)
- // drn = boost::dynamic_pointer_cast<BulletObject>(it->second.dynNode);
- //VR_ASSERT(drn);
+ while (it!=actuationTargets.end())
+ {
+ //BulletObjectPtr drn;
+ //if (it->second.dynNode)
+ // drn = boost::dynamic_pointer_cast<BulletObject>(it->second.dynNode);
+ //VR_ASSERT(drn);
+
+ VelocityMotorController& controller = actuationControllers[it->first];
- if (it->second.node->isRotationalJoint())
+ 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 velocityTarget = it->second.jointVelocityTarget;
+
#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);
- switch (it->second.actuation)
+ if (actuation.mode == 0) {
+ m->m_enableMotor = false;
+ continue;
+ }
+
+ m->m_enableMotor = true;
+
+ if (actuation.modes.position && actuation.modes.velocity)
{
- case ePosition:
- {
- 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?!
- }
- break;
- case eVelocity:
- {
- m->m_enableMotor = true;
- m->m_targetVelocity = it->second.jointVelocityTarget;
- break;
- }
- case eTorque:
- {
- m->m_enableMotor = true;
- m->m_targetVelocity = it->second.jointVelocityTarget;
- break;
- }
- case ePositionVelocity:
- {
- btScalar pos = btScalar(getJointAngle(it->first));
- float gain = 0.5;
- m->m_targetVelocity = it->second.jointVelocityTarget + gain*(it->second.jointPositionTarget - pos) / dt;
- }
- default:
- m->m_enableMotor = false;
+ m->m_targetVelocity = controller.update(posTarget - posActual, velocityTarget, actuation, dt);
}
-#else
- boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
- if (!hinge)
+ else if (actuation.modes.position)
{
- // 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);
- switch (it->second.actuation)
- {
- case ePosition:
- {
- 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)*bulletMotorVelFactor;
- }
- break;
+ m->m_targetVelocity = controller.update(posTarget - posActual, 0, actuation, dt);
+ }
+ else if (actuation.modes.velocity)
+ {
+ m->m_targetVelocity = controller.update(0, velocityTarget, actuation, dt);
+ }
- case eVelocity:
- {
- m->m_enableMotor = true;
- m->m_targetVelocity = it->second.jointVelocityTarget;
- break;
- }
- case eTorque:
- {
- m->m_enableMotor = true;
- m->m_targetVelocity = it->second.jointVelocityTarget;
- break;
- }
- case ePositionVelocity:
- {
- btScalar pos = btScalar(getJointAngle(it->first));
- float gain = 0.5;
- m->m_targetVelocity = it->second.jointVelocityTarget + gain*(it->second.jointValueTarget - pos) / dt;
- break;
- }
+ // FIXME torque based control is ignored
+#else
+ boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint);
- default:
- m->m_enableMotor = false;
- }
- } else
+ if (actuation.mode == 0) {
+ hinge->enableMotor(false);
+ continue;
+ }
+
+ if (it->second.node->getName() == "LeftLeg_Joint3")
{
- switch (it->second.actuation)
- {
- case ePosition:
- {
- 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)*bulletMotorVelFactor,bulletMaxMotorImulse);
- //hinge->enableMotor(true);
- //hinge->setMotorTarget(it->second.jointValueTarget+link.jointValueOffset,dt);
- break;
- }
- case eVelocity:
- {
- hinge->enableAngularMotor(true,it->second.jointVelocityTarget,bulletMaxMotorImulse);
+ std::cout << "Mode: " << (int) actuation.mode << " : " << posTarget << " " << posActual << std::endl;
+ controller.debug();
+ }
- jointCounter++;
- //cout << "jointVelocityTarget for joint " << it->second.node->getName() << " :" << it->second.jointVelocityTarget << endl;
+ if (actuation.modes.position && actuation.modes.velocity)
+ {
+ hinge->enableAngularMotor(true,
+ controller.update(posTarget - posActual, velocityTarget, actuation, dt),
+ bulletMaxMotorImulse);
+ }
+ else if (actuation.modes.position)
+ {
+ hinge->enableAngularMotor(true,
+ controller.update(posTarget - posActual, 0.0, actuation, dt),
+ bulletMaxMotorImulse);
+ }
+ else if (actuation.modes.velocity)
+ {
+ hinge->enableAngularMotor(true,
+ controller.update(0.0, velocityTarget, actuation, dt),
+ bulletMaxMotorImulse);
+ }
+ // FIXME this bypasses the controller (and doesn't work..)
+ else if (actuation.modes.torque)
+ {
+ //Only first try (using torques as velocity targets...)
+ hinge->enableAngularMotor(true,it->second.jointTorqueTarget,bulletMaxMotorImulse);
+ //cout << "jointTorqueTarget for joint " << it->second.node->getName() << " :" << it->second.jointTorqueTarget << endl;
- break;
- }
- case eTorque:
- {
+ /*
+
+ //=======
+ //Here is some code that sets torques directly to the finger joints (bypassing the Bullet motors).
+ //Unfortunately, this does not seem to work, so far.
+ //1. With hinge->enableAngularMotor(true,...), the fingers do not move at all.
+ //2. Wtih hinge->enableAngularMotor(false,...), the fingers are simply actuated by gravity...
+ //=======
- //Only first try (using torques as velocity targets...)
- hinge->enableAngularMotor(true,it->second.jointTorqueTarget,bulletMaxMotorImulse);
- //cout << "jointTorqueTarget for joint " << it->second.node->getName() << " :" << it->second.jointTorqueTarget << endl;
+ //cout << " === == === === === > BulletRobot (hinge !): eTorque NEW!!! ====" << endl;
- /*
+ cout << "Disabling Angular Motor... " << endl;
+ hinge->enableAngularMotor(false,0,bulletMaxMotorImulse);
- //=======
- //Here is some code that sets torques directly to the finger joints (bypassing the Bullet motors).
- //Unfortunately, this does not seem to work, so far.
- //1. With hinge->enableAngularMotor(true,...), the fingers do not move at all.
- //2. Wtih hinge->enableAngularMotor(false,...), the fingers are simply actuated by gravity...
- //=======
- //cout << " === == === === === > BulletRobot (hinge !): eTorque NEW!!! ====" << endl;
+ cout << "=== === === jointCounter: " << jointCounter << " === === ===" << endl;
- cout << "Disabling Angular Motor... " << endl;
- hinge->enableAngularMotor(false,0,bulletMaxMotorImulse);
+ //get the links that are connected by the hinge.
+ btRigidBody rbA = hinge->getRigidBodyA();
+ btRigidBody rbB = hinge->getRigidBodyB();
+ //get joint axis from the hinge ...
+ btMatrix3x3 rbAFrameBasis = hinge->getAFrame().getBasis();
+ //z-Achse ist Gelenkachse? (das steht in btHingeConstraint.h; setAxis() bzw. struct btHingeConstraintDoubleData)
+ btVector3 hingeAxis = rbAFrameBasis.getColumn(2);
- cout << "=== === === jointCounter: " << jointCounter << " === === ===" << endl;
+ //calc 3dim torque by multiplication with joint axis!
+ btVector3 resTorqueA = hingeAxis * it->second.jointTorqueTarget;
- //get the links that are connected by the hinge.
- btRigidBody rbA = hinge->getRigidBodyA();
- btRigidBody rbB = hinge->getRigidBodyB();
- //get joint axis from the hinge ...
- btMatrix3x3 rbAFrameBasis = hinge->getAFrame().getBasis();
- //z-Achse ist Gelenkachse? (das steht in btHingeConstraint.h; setAxis() bzw. struct btHingeConstraintDoubleData)
- btVector3 hingeAxis = rbAFrameBasis.getColumn(2);
+ //TODO (maybe): calc "realistic" torque to be applied (using dt)
- //calc 3dim torque by multiplication with joint axis!
- btVector3 resTorqueA = hingeAxis * it->second.jointTorqueTarget;
+ //apply torques to the bodies connected by the joint
+ rbA.applyTorqueImpulse(resTorqueA);
+ rbB.applyTorqueImpulse(-resTorqueA);
+ //DEBUG OUT:
+ //--> TODO!
+ //cout << "==== ==== ==== DEBUG OUT: ==== ==== ==== " << endl;
+ cout << "rbAFrameBasis:" << endl;
+ btVector3 row0 = rbAFrameBasis.getRow(0);
+ btVector3 row1 = rbAFrameBasis.getRow(1);
+ btVector3 row2 = rbAFrameBasis.getRow(2);
+ cout << row0.getX() << " " << row0.getY() << " " << row0.getZ() << endl;
+ cout << row1.getX() << " " << row1.getY() << " " << row1.getZ() << endl;
+ cout << row2.getX() << " " << row2.getY() << " " << row2.getZ() << endl;
- //TODO (maybe): calc "realistic" torque to be applied (using dt)
+ cout << "hingeAxis: " << hingeAxis.getX() << " " << hingeAxis.getY() << " " << hingeAxis.getZ() << endl;
+ cout << "resTorqueA: " << resTorqueA.getX() << " " << resTorqueA.getY() << " " << resTorqueA.getZ() << endl;
- //apply torques to the bodies connected by the joint
- rbA.applyTorqueImpulse(resTorqueA);
- rbB.applyTorqueImpulse(-resTorqueA);
- //DEBUG OUT:
- //--> TODO!
- //cout << "==== ==== ==== DEBUG OUT: ==== ==== ==== " << endl;
- cout << "rbAFrameBasis:" << endl;
- btVector3 row0 = rbAFrameBasis.getRow(0);
- btVector3 row1 = rbAFrameBasis.getRow(1);
- btVector3 row2 = rbAFrameBasis.getRow(2);
- cout << row0.getX() << " " << row0.getY() << " " << row0.getZ() << endl;
- cout << row1.getX() << " " << row1.getY() << " " << row1.getZ() << endl;
- cout << row2.getX() << " " << row2.getY() << " " << row2.getZ() << endl;
- cout << "hingeAxis: " << hingeAxis.getX() << " " << hingeAxis.getY() << " " << hingeAxis.getZ() << endl;
- cout << "resTorqueA: " << resTorqueA.getX() << " " << resTorqueA.getY() << " " << resTorqueA.getZ() << endl;
+ jointCounter++;
+*/
+ }
+ // Universal constraint instead of hinge constraint
+ /*
+ boost::shared_ptr<btUniversalConstraint> hinge = 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);
+ switch (it->second.actuation)
+ {
+ case ePosition:
+ {
+ 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)*bulletMotorVelFactor;
+ }
+ break;
- jointCounter++;
+ case eVelocity:
+ {
+ m->m_enableMotor = true;
+ m->m_targetVelocity = it->second.jointVelocityTarget;
+ break;
+ }
+ case eTorque:
+ {
+ m->m_enableMotor = true;
+ m->m_targetVelocity = it->second.jointVelocityTarget;
+ break;
+ }
+ case ePositionVelocity:
+ {
+ btScalar pos = btScalar(getJointAngle(it->first));
+ float gain = 0.5;
+ m->m_targetVelocity = it->second.jointVelocityTarget + gain*(it->second.jointValueTarget - pos) / dt;
+ break;
+ }
- */
- break;
- }
- case ePositionVelocity:
- {
- btScalar pos = btScalar(getJointAngle(it->first));
- float gain = 0.5;
- float target = it->second.jointVelocityTarget + gain*(it->second.jointValueTarget - pos) / dt;
- hinge->enableAngularMotor(true, target, bulletMaxMotorImulse);
-
- jointCounter++;
- break;
- }
-
- default:
- hinge->enableMotor(false);
- }
+ default:
+ m->m_enableMotor = false;
}
+ */
#endif
- }
+ }
- it++;
- }
+ it++;
+ }
setPoseNonActuatedRobotNodes();
}
diff --git a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
index 569109a2c..e106522f6 100644
--- a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
+++ b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
@@ -90,14 +90,19 @@ void DynamicsRobot::actuateNode( VirtualRobot::RobotNodePtr node, float jointVal
robotNodeActuationTarget target;
- target.actuation = ePosition;
+ target.actuation.modes.position = 1;
target.node = node;
target.jointValueTarget = jointValue;
- target.jointVelocityTarget = 0.0f;
- target.jointTorqueTarget = 0.0f;
- //target.dynNode = dnyRN;
actuationTargets[node] = target;
+ if (actuationControllers.find(node) == actuationControllers.end())
+ {
+ actuationControllers[node] = VelocityMotorController();
+ }
+ else
+ {
+ actuationControllers[node].reset();
+ }
}
void DynamicsRobot::actuateNode( VirtualRobot::RobotNodePtr node, float jointValue , float jointVelocity)
@@ -107,20 +112,29 @@ void DynamicsRobot::actuateNode( VirtualRobot::RobotNodePtr node, float jointVal
VR_ASSERT(robot->hasRobotNode(node));
robotNodeActuationTarget target;
- target.actuation = ePositionVelocity;
+ target.actuation.modes.position = 1;
+ target.actuation.modes.velocity = 1;
target.node = node;
target.jointValueTarget = jointValue;
target.jointVelocityTarget = jointVelocity;
- target.jointTorqueTarget = 0.0f;
actuationTargets[node] = target;
+ if (actuationControllers.find(node) == actuationControllers.end())
+ {
+ actuationControllers[node] = VelocityMotorController();
+ }
+ else
+ {
+ actuationControllers[node].reset();
+ }
}
void DynamicsRobot::actuateNodeVel(const std::string &node, float jointVelocity )
{
VR_ASSERT(robot);
VR_ASSERT(robot->hasRobotNode(node));
- actuateNodeVel(robot->getRobotNode(node),jointVelocity);
+
+ actuateNodeVel(robot->getRobotNode(node), jointVelocity);
}
void DynamicsRobot::actuateNodeVel( VirtualRobot::RobotNodePtr node, float jointVelocity )
@@ -135,14 +149,19 @@ void DynamicsRobot::actuateNodeVel( VirtualRobot::RobotNodePtr node, float joint
//DynamicsObjectPtr dnyRN = getDynamicsRobotNode(node);
robotNodeActuationTarget target;
- target.actuation = eVelocity;
+ target.actuation.modes.velocity = 1;
target.node = node;
- target.jointValueTarget = 0.0f;
target.jointVelocityTarget = jointVelocity;
- target.jointTorqueTarget = 0.0f;
- //target.dynNode = dnyRN;
actuationTargets[node] = target;
+ if (actuationControllers.find(node) == actuationControllers.end())
+ {
+ actuationControllers[node] = VelocityMotorController();
+ }
+ else
+ {
+ actuationControllers[node].reset();
+ }
}
void DynamicsRobot::actuateNodeTorque(const std::string &node, float jointTorque )
@@ -164,14 +183,20 @@ void DynamicsRobot::actuateNodeTorque( VirtualRobot::RobotNodePtr node, float jo
//DynamicsObjectPtr dnyRN = getDynamicsRobotNode(node);
robotNodeActuationTarget target;
- target.actuation = eTorque;
+ target.actuation.modes.torque = 1;
target.node = node;
- target.jointValueTarget = 0.0f;
- target.jointVelocityTarget = 0.0f;
target.jointTorqueTarget = jointTorque;
//target.dynNode = dnyRN;
actuationTargets[node] = target;
+ if (actuationControllers.find(node) == actuationControllers.end())
+ {
+ actuationControllers[node] = VelocityMotorController();
+ }
+ else
+ {
+ actuationControllers[node].reset();
+ }
}
void DynamicsRobot::disableNodeActuation( VirtualRobot::RobotNodePtr node )
@@ -181,7 +206,7 @@ void DynamicsRobot::disableNodeActuation( VirtualRobot::RobotNodePtr node )
actuationTargets.erase(node);
}
}
-void DynamicsRobot::enableActuation(JointActuation mode)
+void DynamicsRobot::enableActuation(ActuationMode mode)
{
std::map<VirtualRobot::RobotNodePtr, robotNodeActuationTarget>::iterator it = actuationTargets.begin();
while (it!=actuationTargets.end())
@@ -195,7 +220,7 @@ void DynamicsRobot::disableActuation()
std::map<VirtualRobot::RobotNodePtr, robotNodeActuationTarget>::iterator it = actuationTargets.begin();
while (it!=actuationTargets.end())
{
- it->second.actuation = eDisabled;
+ it->second.actuation.mode = 0;
it++;
}
}
@@ -209,7 +234,7 @@ bool DynamicsRobot::isNodeActuated( VirtualRobot::RobotNodePtr node )
VR_ASSERT(node);
if (actuationTargets.find(node) == actuationTargets.end())
return false;
- return actuationTargets[node].actuation!=eDisabled;
+ return actuationTargets[node].actuation.mode != 0;
}
float DynamicsRobot::getNodeTarget( VirtualRobot::RobotNodePtr node )
diff --git a/SimDynamics/DynamicsEngine/DynamicsRobot.h b/SimDynamics/DynamicsEngine/DynamicsRobot.h
index 9d13bbbdd..0f93b39c8 100644
--- a/SimDynamics/DynamicsEngine/DynamicsRobot.h
+++ b/SimDynamics/DynamicsEngine/DynamicsRobot.h
@@ -25,6 +25,7 @@
#include "../SimDynamics.h"
#include "DynamicsObject.h"
+#include "DynamicsUtils.h"
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/Nodes/Sensor.h>
@@ -58,15 +59,6 @@ public:
*/
DynamicsObjectPtr getDynamicsRobotNode(VirtualRobot::RobotNodePtr node);
- enum JointActuation
- {
- eDisabled,
- ePosition,
- eVelocity,
- ePositionVelocity,
- eTorque
- };
-
/*!
Enable joint actuation for given node.
*/
@@ -80,7 +72,7 @@ public:
virtual void disableNodeActuation(VirtualRobot::RobotNodePtr node);
virtual bool isNodeActuated(VirtualRobot::RobotNodePtr node);
virtual float getNodeTarget(VirtualRobot::RobotNodePtr node);
- virtual void enableActuation(JointActuation mode);
+ virtual void enableActuation(ActuationMode mode);
virtual void disableActuation();
/*!
@@ -108,8 +100,6 @@ public:
virtual void setGlobalPose(Eigen::Matrix4f &gp);
-
-
protected:
virtual void createDynamicsNode(VirtualRobot::RobotNodePtr node);
@@ -117,15 +107,23 @@ protected:
struct robotNodeActuationTarget
{
+ robotNodeActuationTarget()
+ : jointValueTarget(0)
+ , jointVelocityTarget(0)
+ , jointTorqueTarget(0)
+ {
+ actuation.mode = 0;
+ }
float jointValueTarget;
float jointVelocityTarget;
float jointTorqueTarget;
VirtualRobot::RobotNodePtr node;
//DynamicsObjectPtr dynNode; // if node is a joint without model, there is no dyn node!
- JointActuation actuation;
+ ActuationMode actuation;
};
std::map<VirtualRobot::RobotNodePtr, robotNodeActuationTarget> actuationTargets;
+ std::map<VirtualRobot::RobotNodePtr, VelocityMotorController> actuationControllers;
VirtualRobot::RobotPtr robot;
diff --git a/SimDynamics/DynamicsEngine/DynamicsUtils.cpp b/SimDynamics/DynamicsEngine/DynamicsUtils.cpp
new file mode 100644
index 000000000..6ef55800f
--- /dev/null
+++ b/SimDynamics/DynamicsEngine/DynamicsUtils.cpp
@@ -0,0 +1,133 @@
+
+#include "DynamicsUtils.h"
+
+#include <iostream>
+
+namespace SimDynamics {
+PIDController::PIDController(float gainP, float gainI, float gainD)
+: gainP(gainP)
+, gainI(gainI)
+, gainD(gainD)
+, errorSum(0)
+, lastError(0)
+{
+}
+
+float PIDController::update(float error, float dt)
+{
+ float p = error * gainP;
+ errorSum += error * dt;
+ float i = errorSum * gainI;
+ float d = (error - lastError)/dt * gainD;
+ lastError = error;
+
+ float output = (p + i + d);
+ lastOutput = output;
+ return output;
+}
+
+void PIDController::reset()
+{
+ errorSum = 0.0;
+ lastError = 0.0;
+}
+
+void PIDController::debug()
+{
+ std::cout << "error sum: " << errorSum
+ << " last error: " << lastError
+ << " last output: " << lastOutput
+ << std::endl;
+}
+
+TorqueMotorController::TorqueMotorController()
+: positionController(0.5, 0.05, 0.0)
+, velocityController(1.0, 0.05, 0.0)
+, torqueController(1.0, 0.05, 0.0)
+{
+}
+
+TorqueMotorController::TorqueMotorController(const PIDController& positionController,
+ const PIDController& velocityController,
+ const PIDController& torqueController)
+: positionController(positionController)
+, velocityController(velocityController)
+, torqueController(torqueController)
+{
+}
+
+
+float TorqueMotorController::update(float positionError, float velocityError, float torqueError, ActuationMode actuation, float dt)
+{
+ float posUpdate = 0.0;
+ float velUpdate = 0.0;
+ float torqueUpdate = 0.0;
+ if (actuation.modes.position)
+ posUpdate = positionController.update(positionError, dt);
+ else
+ positionController.reset();
+
+ if (actuation.modes.velocity)
+ velUpdate = velocityController.update(velocityError + posUpdate, dt);
+ else
+ velocityController.reset();
+
+ if (actuation.modes.torque)
+ torqueUpdate = torqueController.update(torqueError + velUpdate, dt);
+ else
+ torqueController.reset();
+
+ if (actuation.modes.position && actuation.modes.velocity && actuation.modes.torque)
+ return torqueUpdate;
+ if (actuation.modes.position && actuation.modes.velocity)
+ return velUpdate;
+ if (actuation.modes.position)
+ return posUpdate;
+ if (actuation.modes.velocity)
+ return velUpdate;
+ if (actuation.modes.torque)
+ return torqueUpdate;
+
+ return 0.0f;
+}
+
+VelocityMotorController::VelocityMotorController()
+: positionController(100.0, 50.0, 0.0)
+{
+}
+
+VelocityMotorController::VelocityMotorController(const PIDController& positionController)
+: positionController(positionController)
+{
+}
+
+
+float VelocityMotorController::update(float positionError, float targetVelocity, ActuationMode actuation, float dt)
+{
+ float posUpdate = 0.0;
+ if (actuation.modes.position)
+ posUpdate = positionController.update(positionError, dt);
+ else
+ positionController.reset();
+
+ float output = 0.0f;
+ if (actuation.modes.position && actuation.modes.velocity)
+ output = posUpdate + targetVelocity;
+ else if (actuation.modes.position)
+ output = posUpdate;
+ else if (actuation.modes.velocity)
+ output = targetVelocity;
+
+ return output;
+}
+
+void VelocityMotorController::reset()
+{
+ positionController.reset();
+}
+
+void VelocityMotorController::debug()
+{
+ positionController.debug();
+}
+}
diff --git a/SimDynamics/DynamicsEngine/DynamicsUtils.h b/SimDynamics/DynamicsEngine/DynamicsUtils.h
new file mode 100644
index 000000000..d5d3d5686
--- /dev/null
+++ b/SimDynamics/DynamicsEngine/DynamicsUtils.h
@@ -0,0 +1,113 @@
+#ifndef __DYNAMICS_UTILS__H__
+#define __DYNAMICS_UTILS__H__
+
+#include <iostream>
+
+namespace SimDynamics {
+class PIDController {
+public:
+ PIDController(float gainP, float gainI, float gainD);
+
+ float update(float error, float dt);
+
+ void reset();
+
+ void debug();
+
+private:
+ float gainP;
+ float gainI;
+ float gainD;
+ float errorSum;
+ float lastError;
+ float lastOutput;
+};
+
+// use bit field because enums are a pain
+union ActuationMode {
+ struct {
+ unsigned char position:1;
+ unsigned char velocity:1;
+ unsigned char torque:1;
+ } modes;
+ unsigned char mode;
+};
+
+/**
+ * For *torque* based motors.
+ *
+ * Position only:
+ * position error --> [PID] --> joint
+ *
+ * Velocity only:
+ * velocity error --> [PID] --> joint
+ *
+ * Torque only:
+ * torque error --> [PID] --> joint
+ *
+ * Position + Velocity:
+ * velocity error
+ * |
+ * v
+ * position error -> [PID] -> (+) -> [PID] -> joint
+ *
+ * Position + Velocity + Torque:
+ * velocity error torque error
+ * | |
+ * v v
+ * position error -> [PID] -> (+) -> [PID] -> (+) -> [PID] -> joint
+ *
+ */
+class TorqueMotorController{
+public:
+ TorqueMotorController();
+ TorqueMotorController(const PIDController& positionController,
+ const PIDController& velocityController,
+ const PIDController& torqueController);
+
+ float update(float positionError, float velocityError, float torqueError, ActuationMode actuation, float dt);
+
+private:
+ PIDController positionController;
+ PIDController velocityController;
+ PIDController torqueController;
+};
+
+/**
+ * For *velocity* based motors (where target velocity == actual velocity).
+ * We use this for Bullet motors.
+ *
+ * Note: Torque is ignored. This controler returns *velocities*.
+ *
+ * Position only:
+ * position error --> [PID] --> joint
+ *
+ * Velocity only:
+ * target velocity --> joint
+ *
+ * Position + Velocity:
+ * target velocity
+ * |
+ * v
+ * position error -> [PID] -> (+) -> joint
+ *
+ */
+class VelocityMotorController {
+public:
+ VelocityMotorController();
+
+ VelocityMotorController(const PIDController& positionController);
+
+
+ float update(float positionError, float targetVelocity, ActuationMode actuation, float dt);
+
+ void reset();
+
+ void debug();
+
+private:
+ PIDController positionController;
+};
+}
+
+#endif
diff --git a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
index c6623f99c..c71c0ad82 100644
--- a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
+++ b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
@@ -173,9 +173,15 @@ void SimDynamicsWindow::actuation()
bool actuate = UI.checkBoxActuation->checkState() == Qt::Checked;
if (actuate)
- dynamicsRobot->enableActuation(SimDynamics::DynamicsRobot::ePosition);
+ {
+ ActuationMode actuation;
+ actuation.modes.position = 1;
+ dynamicsRobot->enableActuation(actuation);
+ }
else
+ {
dynamicsRobot->disableActuation();
+ }
}
void SimDynamicsWindow::buildVisualization()
--
GitLab