From cc8b49256b97cab901280edd8c5b1f4bd9b9c7b5 Mon Sep 17 00:00:00 2001
From: themarex <themarex@042f3d55-54a8-47e9-b7fb-15903f145c44>
Date: Sun, 26 Oct 2014 21:26:29 +0000
Subject: [PATCH] Add methode to compute angular momentum around CoM
git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@718 042f3d55-54a8-47e9-b7fb-15903f145c44
---
.../BulletEngine/BulletRobot.cpp | 29 +++++++++++++++++++
.../DynamicsEngine/BulletEngine/BulletRobot.h | 5 ++++
SimDynamics/DynamicsEngine/DynamicsRobot.cpp | 5 ++++
SimDynamics/DynamicsEngine/DynamicsRobot.h | 1 +
4 files changed, 40 insertions(+)
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
index 49778717f..6eb2bd0b1 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
@@ -1248,6 +1248,35 @@ Eigen::Vector3f BulletRobot::getAngularMomentumGlobal(const VirtualRobot::RobotN
return angMomentum;
}
+Eigen::Vector3f BulletRobot::getAngularMomentumLocal(const VirtualRobot::RobotNodeSetPtr& set)
+{
+ MutexLockPtr lock = getScopedLock();
+ Eigen::Vector3f angMomentum = Eigen::Vector3f::Zero();
+ Eigen::Vector3f com = getComGlobal(set) / 1000.0;
+ Eigen::Vector3f comVel = getComVelocityGlobal(set) / 1000;
+ for (unsigned int i = 0; i < set->getSize(); i++)
+ {
+ VirtualRobot::RobotNodePtr node = (*set)[i];
+ BulletObjectPtr bo = boost::dynamic_pointer_cast<BulletObject>(getDynamicsRobotNode(node));
+ Eigen::Vector3f bodyVel = bo->getLinearVelocity() / 1000.0;
+ Eigen::Vector3f ang = bo->getAngularVelocity() / 1000.0;
+ Eigen::Vector3f bodyCoM = bo->getComGlobal().block(0, 3, 3, 1) / 1000.0;
+ double mass = node->getMass();
+
+ boost::shared_ptr<btRigidBody> body = bo->getRigidBody();
+
+ btVector3 invIntertiaDiag = body->getInvInertiaDiagLocal();
+ Eigen::Matrix3f intertiaLocal = Eigen::Matrix3f::Zero();
+ intertiaLocal(0, 0) = 1 / invIntertiaDiag.getX();
+ intertiaLocal(1, 1) = 1 / invIntertiaDiag.getY();
+ intertiaLocal(2, 2) = 1 / invIntertiaDiag.getZ();
+
+ angMomentum += mass * (bodyCoM - com).cross(bodyVel - comVel) + intertiaLocal * ang;
+ }
+
+ return angMomentum;
+}
+
void BulletRobot::setPoseNonActuatedRobotNodes()
{
MutexLockPtr lock = getScopedLock();
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
index 43be94cf9..6a05eed83 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
@@ -162,6 +162,11 @@ public:
*/
virtual Eigen::Vector3f getAngularMomentumGlobal(const VirtualRobot::RobotNodeSetPtr& set);
+ /*!
+ * Returns the angular momentum in Nms for the bodies in the nodeset relative to the CoM
+ */
+ virtual Eigen::Vector3f getAngularMomentumLocal(const VirtualRobot::RobotNodeSetPtr& set);
+
// experimental...
virtual void ensureKinematicConstraints();
diff --git a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
index b969c67da..d1645b788 100644
--- a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
+++ b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
@@ -315,6 +315,11 @@ Eigen::Vector3f DynamicsRobot::getAngularMomentumGlobal(const VirtualRobot::Robo
return Eigen::Vector3f::Zero();
}
+Eigen::Vector3f DynamicsRobot::getAngularMomentumLocal(const VirtualRobot::RobotNodeSetPtr& set)
+{
+ return Eigen::Vector3f::Zero();
+}
+
void DynamicsRobot::setGlobalPose(Eigen::Matrix4f &gp)
{
MutexLockPtr lock = getScopedLock();
diff --git a/SimDynamics/DynamicsEngine/DynamicsRobot.h b/SimDynamics/DynamicsEngine/DynamicsRobot.h
index cc8d8ab41..cd30e42f3 100644
--- a/SimDynamics/DynamicsEngine/DynamicsRobot.h
+++ b/SimDynamics/DynamicsEngine/DynamicsRobot.h
@@ -104,6 +104,7 @@ public:
virtual Eigen::Vector3f getLinearMomentumGlobal(const VirtualRobot::RobotNodeSetPtr& set);
virtual Eigen::Vector3f getAngularMomentumGlobal(const VirtualRobot::RobotNodeSetPtr& set);
+ virtual Eigen::Vector3f getAngularMomentumLocal(const VirtualRobot::RobotNodeSetPtr& set);
virtual void setGlobalPose(Eigen::Matrix4f &gp);
--
GitLab