diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp index fa6731bad6a037690954716aa186f9024d818056..36d2d5f391dfb019588e4e48a063550729c1b6f9 100644 --- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp +++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp @@ -1218,7 +1218,7 @@ Eigen::Vector3f BulletRobot::getLinearMomentumGlobal( VirtualRobot::RobotNodeSet { VirtualRobot::RobotNodePtr node = (*set)[i]; BulletObjectPtr bo = boost::dynamic_pointer_cast<BulletObject>(getDynamicsRobotNode(node)); - Eigen::Vector3f vel = bo->getLinearVelocity(); + Eigen::Vector3f vel = bo->getLinearVelocity()/1000.0; linMomentum += node->getMass() * vel; } @@ -1234,9 +1234,9 @@ Eigen::Vector3f BulletRobot::getAngularMomentumGlobal( VirtualRobot::RobotNodeSe { VirtualRobot::RobotNodePtr node = (*set)[i]; BulletObjectPtr bo = boost::dynamic_pointer_cast<BulletObject>(getDynamicsRobotNode(node)); - Eigen::Vector3f vel = bo->getLinearVelocity(); - Eigen::Vector3f ang = bo->getAngularVelocity(); - Eigen::Vector3f com = bo->getComGlobal().block(0, 3, 3, 1); + Eigen::Vector3f vel = bo->getLinearVelocity() / 1000.0; + Eigen::Vector3f ang = bo->getAngularVelocity() / 1000.0; + Eigen::Vector3f com = bo->getComGlobal().block(0, 3, 3, 1) / 1000.0; double mass = node->getMass(); boost::shared_ptr<btRigidBody> body = bo->getRigidBody(); diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h index 1c439b188df165087dc0bcd8ffbdb1ce60c5b2b7..0d69bfb6db5b1e007407ee08fdf1f0774e34a3a7 100644 --- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h +++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h @@ -151,7 +151,15 @@ public: virtual Eigen::Matrix4f getComGlobal(VirtualRobot::RobotNodePtr rn); Eigen::Vector3f getComGlobal(VirtualRobot::RobotNodeSetPtr set); Eigen::Vector3f getComVelocityGlobal(VirtualRobot::RobotNodeSetPtr set); + + /*! + * Returns the linear momentum in Ns for the bodies in the nodeset. + */ Eigen::Vector3f getLinearMomentumGlobal(VirtualRobot::RobotNodeSetPtr set); + + /*! + * Returns the angular momentum in Nms for the bodies in the nodeset + */ Eigen::Vector3f getAngularMomentumGlobal(VirtualRobot::RobotNodeSetPtr set); // experimental...