diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp index c186e669e68bb668b8ce5a670ebe38df0e131d0f..fd5bdab848068538f0e6d4c1844d26aec531afe6 100644 --- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp +++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp @@ -28,7 +28,7 @@ namespace SimDynamics { BulletRobot::BulletRobot(VirtualRobot::RobotPtr rob, bool enableJointMotors) : DynamicsRobot(rob) { - bulletMaxMotorImulse = 1500.0f; + bulletMaxMotorImulse = 100.0f; bulletMotorVelFactor = 100.0f; buildBulletModels(enableJointMotors); diff --git a/SimDynamics/DynamicsEngine/DynamicsUtils.cpp b/SimDynamics/DynamicsEngine/DynamicsUtils.cpp index 564f780d21a9ab143351244858e52f4bdc41d513..0cfa191c5aa4748375ffe93557490b55a0987ffe 100644 --- a/SimDynamics/DynamicsEngine/DynamicsUtils.cpp +++ b/SimDynamics/DynamicsEngine/DynamicsUtils.cpp @@ -2,6 +2,7 @@ #include "DynamicsUtils.h" #include <iostream> +#include <cmath> namespace SimDynamics { PIDController::PIDController(double gainP, double gainI, double gainD) @@ -92,7 +93,9 @@ double TorqueMotorController::update(double positionError, double velocityError, } VelocityMotorController::VelocityMotorController() -: positionController(100.0, 10.0, 0.0) +: positionController(80.0, 10.0, 0.8) +, maxAcceleration(80.0) +, velocity(0) { } @@ -118,6 +121,14 @@ double VelocityMotorController::update(double positionError, double targetVeloci else if (actuation.modes.velocity) output = targetVelocity; + if (fabs(output - velocity)/dt > maxAcceleration) + { + double sign = (output - velocity) > 0 ? 1.0 : -1.0; + output = velocity + sign * maxAcceleration*dt; + } + + velocity = output; + return output; } diff --git a/SimDynamics/DynamicsEngine/DynamicsUtils.h b/SimDynamics/DynamicsEngine/DynamicsUtils.h index 52620d2dde5ab278fa6000e515974f2c671c25c1..44f29a6a835e4e810c0919c8ebf2775ef5c2b0db 100644 --- a/SimDynamics/DynamicsEngine/DynamicsUtils.h +++ b/SimDynamics/DynamicsEngine/DynamicsUtils.h @@ -107,6 +107,8 @@ public: private: PIDController positionController; + double maxAcceleration; + double velocity; }; }