From ae170961881364c2ae8a6ba221b20452281aa44b Mon Sep 17 00:00:00 2001
From: vahrenkamp <vahrenkamp@042f3d55-54a8-47e9-b7fb-15903f145c44>
Date: Tue, 15 Jul 2014 11:47:03 +0000
Subject: [PATCH] Added maxVelocity support
git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@622 042f3d55-54a8-47e9-b7fb-15903f145c44
---
.../BulletEngine/BulletRobot.cpp | 44 ++++++++++++++++---
.../DynamicsEngine/BulletEngine/BulletRobot.h | 2 -
2 files changed, 39 insertions(+), 7 deletions(-)
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
index fd5bdab84..be89e8348 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
@@ -30,7 +30,6 @@ BulletRobot::BulletRobot(VirtualRobot::RobotPtr rob, bool enableJointMotors)
{
bulletMaxMotorImulse = 100.0f;
- bulletMotorVelFactor = 100.0f;
buildBulletModels(enableJointMotors);
// activate force torque sensors
@@ -744,6 +743,7 @@ void BulletRobot::actuateJoints(double dt)
std::map<VirtualRobot::RobotNodePtr, robotNodeActuationTarget>::iterator it = actuationTargets.begin();
int jointCounter = 0;
+ //cout << "**** Control Values: ";
for (; it != actuationTargets.end(); it++)
{
@@ -798,19 +798,47 @@ void BulletRobot::actuateJoints(double dt)
hinge->enableMotor(false);
continue;
}
-
+ double controllerOutput;
double targetVelocity;
if (actuation.modes.position && actuation.modes.velocity)
{
targetVelocity = controller.update(posTarget - posActual, velocityTarget, actuation, btScalar(dt));
+ /*if (it->second.node->getMaxVelocity()>0 && fabs(targetVelocity)>it->second.node->getMaxVelocity())
+ {
+ double newOutput = double(it->second.node->getMaxVelocity());
+ targetVelocity = copysign(newOutput,targetVelocity);
+ }
+ hinge->enableAngularMotor(true,
+ controllerOutput,
+ bulletMaxMotorImulse);*/
+
}
else if (actuation.modes.position)
{
- targetVelocity = controller.update(posTarget - posActual, 0.0, actuation, btScalar(dt));
+ targetVelocity = controller.update(posTarget - posActual, 0.0, actuation, btScalar(dt));
+ /*if (it->second.node->getMaxVelocity()>0 && fabs(targetVelocity)>it->second.node->getMaxVelocity())
+ {
+ double newOutput = double(it->second.node->getMaxVelocity());
+ controllerOutput = copysign(newOutput,controllerOutput);
+ }
+ hinge->enableAngularMotor(true,
+ controllerOutput,
+ bulletMaxMotorImulse);*/
+
}
else if (actuation.modes.velocity)
{
targetVelocity = controller.update(0.0, velocityTarget, actuation, btScalar(dt));
+ /*if (it->second.node->getMaxVelocity()>0 && fabs(controllerOutput)>it->second.node->getMaxVelocity())
+ {
+ double newOutput = double(it->second.node->getMaxVelocity());
+ controllerOutput = copysign(newOutput,controllerOutput);
+ }
+ hinge->enableAngularMotor(true,
+ controllerOutput,
+ bulletMaxMotorImulse);*/
+
+ targetVelocity = controller.update(0.0, velocityTarget, actuation, btScalar(dt));
}
// FIXME this bypasses the controller (and doesn't work..)
else if (actuation.modes.torque)
@@ -874,9 +902,14 @@ void BulletRobot::actuateJoints(double dt)
*/
}
-
+ if (it->second.node->getMaxVelocity()>0 && fabs(targetVelocity)>it->second.node->getMaxVelocity())
+ {
+ double newOutput = double(it->second.node->getMaxVelocity());
+ targetVelocity = copysign(newOutput,targetVelocity);
+ }
hinge->enableAngularMotor(true, targetVelocity, bulletMaxMotorImulse);
+
// Universal constraint instead of hinge constraint
/*
boost::shared_ptr<btUniversalConstraint> hinge = boost::dynamic_pointer_cast<btUniversalConstraint>(link.joint);
@@ -930,6 +963,7 @@ void BulletRobot::actuateJoints(double dt)
#endif
}
}
+ //cout << endl;
setPoseNonActuatedRobotNodes();
}
@@ -950,7 +984,7 @@ void BulletRobot::updateSensors()
const LinkInfo& link = getLink(node);
Eigen::VectorXf forceTorques = getJointForceTorqueGlobal(link);
ftSensor->updateSensors(forceTorques);
- std::cout << "Updating force torque sensor: " << node->getName() << ": " << forceTorques << std::endl;
+ //std::cout << "Updating force torque sensor: " << node->getName() << ": " << forceTorques << std::endl;
}
}
}
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
index 0d9dee199..5e9048928 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
@@ -185,8 +185,6 @@ protected:
std::vector<LinkInfo> links;
btScalar bulletMaxMotorImulse;
- btScalar bulletMotorVelFactor;
-
};
typedef boost::shared_ptr<BulletRobot> BulletRobotPtr;
--
GitLab