From 2f5c93a0f40a2aa349e36bdaa5daf5a174a9b994 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 23 Jan 2019 10:42:59 -0800 Subject: [PATCH 01/13] PyBullet: enable DeepMimic code as 'plugin' for stable PD control of spherical joints enabling STATIC_LINK_SPD_PLUGIN --- setup.py | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 98efabdbf..9d6bd9033 100644 --- a/setup.py +++ b/setup.py @@ -47,6 +47,8 @@ CXX_FLAGS += '-DEGL_ADD_PYTHON_INIT ' CXX_FLAGS += '-DB3_ENABLE_FILEIO_PLUGIN ' CXX_FLAGS += '-DB3_USE_ZIPFILE_FILEIO ' CXX_FLAGS += '-DBT_THREADSAFE=1 ' +CXX_FLAGS += '-DSTATIC_LINK_SPD_PLUGIN ' + EGL_CXX_FLAGS = '' @@ -98,6 +100,14 @@ sources = ["examples/pybullet/pybullet.c"]\ +["examples/SharedMemory/PosixSharedMemory.cpp"]\ +["examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp"]\ +["examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp"]\ ++["examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp"]\ ++["examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp"]\ ++["examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp"]\ ++["examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp"]\ ++["examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp"]\ ++["examples/SharedMemory/plugins/stablePDPlugin/Rand.cpp"]\ ++["examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp"]\ ++["examples/SharedMemory/plugins/stablePDPlugin/SpAlg.cpp"]\ +["examples/SharedMemory/PhysicsClientUDP.cpp"]\ +["examples/SharedMemory/PhysicsClientUDP_C_API.cpp"]\ +["examples/SharedMemory/PhysicsClientTCP.cpp"]\ @@ -585,7 +595,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS: setup( name = 'pybullet', - version='2.4.0', + version='2.4.2', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', From 7b99810e4e3487e8bde185ad44ba6ce759f46c64 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 23 Jan 2019 12:50:44 -0800 Subject: [PATCH 02/13] fix case sensitive include header name --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 68e3492b3..9e0652adf 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -71,7 +71,7 @@ #ifdef STATIC_LINK_SPD_PLUGIN #include "plugins/stablePDPlugin/BulletConversion.h" #include "plugins/stablePDPlugin/RBDModel.h" -#include "plugins/stablePDPlugin/RBDutil.h" +#include "plugins/stablePDPlugin/RBDUtil.h" #endif #ifdef STATIC_LINK_VR_PLUGIN From 93087f36dfcd03258060bad5d409a8c18fe93d31 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 23 Jan 2019 13:35:58 -0800 Subject: [PATCH 03/13] avoid conflicts when using a unity build (single cpp file including many other cpp) --- .../CollisionShapes/btConvexPolyhedron.cpp | 4 ++-- .../Dynamics/btSimulationIslandManagerMt.cpp | 4 ++-- .../Featherstone/btMultiBodyMLCPConstraintSolver.cpp | 12 ++++++------ 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp index 65b669e1c..d5ec6cb92 100644 --- a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp +++ b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp @@ -27,7 +27,7 @@ btConvexPolyhedron::~btConvexPolyhedron() { } -inline bool IsAlmostZero(const btVector3& v) +inline bool IsAlmostZero1(const btVector3& v) { if (btFabs(v.x()) > 1e-6 || btFabs(v.y()) > 1e-6 || btFabs(v.z()) > 1e-6) return false; return true; @@ -123,7 +123,7 @@ void btConvexPolyhedron::initialize() for (int p = 0; p < m_uniqueEdges.size(); p++) { if (IsAlmostZero(m_uniqueEdges[p] - edge) || - IsAlmostZero(m_uniqueEdges[p] + edge)) + IsAlmostZero1(m_uniqueEdges[p] + edge)) { found = true; break; diff --git a/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp b/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp index 17287aa82..5353fe009 100644 --- a/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp +++ b/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp @@ -65,7 +65,7 @@ inline int getIslandId(const btPersistentManifold* lhs) return islandId; } -SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs) +SIMD_FORCE_INLINE int btGetConstraintIslandId1(const btTypedConstraint* lhs) { const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); @@ -452,7 +452,7 @@ void btSimulationIslandManagerMt::addConstraintsToIslands(btAlignedObjectArrayisEnabled()) { - int islandId = btGetConstraintIslandId(constraint); + int islandId = btGetConstraintIslandId1(constraint); // if island is not sleeping, if (Island* island = getIsland(islandId)) { diff --git a/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp index 338e8af0a..f2186a93e 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp @@ -22,9 +22,9 @@ subject to the following restrictions: #define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS -static bool interleaveContactAndFriction = false; +static bool interleaveContactAndFriction1 = false; -struct btJointNode +struct btJointNode1 { int jointIndex; // pointer to enclosing dxJoint object int otherBodyIndex; // *other* body this joint is connected to @@ -241,7 +241,7 @@ void btMultiBodyMLCPConstraintSolver::createMLCPFast(const btContactSolverInfo& void btMultiBodyMLCPConstraintSolver::createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal) { - int numContactRows = interleaveContactAndFriction ? 3 : 1; + int numContactRows = interleaveContactAndFriction1 ? 3 : 1; int numConstraintRows = m_allConstraintPtrArray.size(); @@ -301,7 +301,7 @@ void btMultiBodyMLCPConstraintSolver::createMLCPFastRigidBody(const btContactSol BT_PROFILE("bodyJointNodeArray.resize"); bodyJointNodeArray.resize(numBodies, -1); } - btAlignedObjectArray jointNodeArray; + btAlignedObjectArray jointNodeArray; { BT_PROFILE("jointNodeArray.reserve"); jointNodeArray.reserve(2 * m_allConstraintPtrArray.size()); @@ -729,7 +729,7 @@ btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlySetup( int firstContactConstraintOffset = dindex; // The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead - if (interleaveContactAndFriction) + if (interleaveContactAndFriction1) { for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++) { @@ -785,7 +785,7 @@ btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlySetup( firstContactConstraintOffset = dindex; // The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead - if (interleaveContactAndFriction) + if (interleaveContactAndFriction1) { for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i) { From a174b42c024976df595e59be82e46e4248d2332b Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 23 Jan 2019 16:33:06 -0800 Subject: [PATCH 04/13] fix compile issues --- .../TinyRendererVisualShapeConverter.cpp | 4 ++-- .../details/MultiBodyTreeImpl.cpp | 10 ++++++++-- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp index 64d385cbe..6fb538089 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp @@ -92,8 +92,8 @@ struct TinyRendererVisualShapeConverterInternalData SimpleCamera m_camera; TinyRendererVisualShapeConverterInternalData() - : m_upAxis(2), - m_uidGenerator(1), + : m_uidGenerator(1), + m_upAxis(2), m_swWidth(START_WIDTH), m_swHeight(START_HEIGHT), m_rgbColorBuffer(START_WIDTH, START_HEIGHT, TGAImage::RGB), diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp index 4b1ba3ba4..ec9a56229 100644 --- a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp +++ b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp @@ -486,7 +486,10 @@ int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx transformZ(q(body.m_q_index + 2)); body.m_body_T_parent = T * body.m_body_T_parent_ref; - body.m_parent_pos_parent_body.setValue(0,0,0); + body.m_parent_pos_parent_body(0)=0; + body.m_parent_pos_parent_body(1)=0; + body.m_parent_pos_parent_body(2)=0; + body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; if (type >= POSITION_VELOCITY) @@ -850,7 +853,10 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool transformZ(q(body.m_q_index + 2)); body.m_body_T_parent = T * body.m_body_T_parent_ref; - body.m_parent_pos_parent_body.setValue(0, 0, 0); + body.m_parent_pos_parent_body(0)=0; + body.m_parent_pos_parent_body(1)=0; + body.m_parent_pos_parent_body(2)=0; + body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; } } From a3ec60da67514c4ba5af8b494441d95e99d3ad99 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 23 Jan 2019 16:45:29 -0800 Subject: [PATCH 05/13] IsAlmostZero -> IsAlmostZero1 --- src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp index d5ec6cb92..9694f4ddb 100644 --- a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp +++ b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp @@ -122,7 +122,7 @@ void btConvexPolyhedron::initialize() for (int p = 0; p < m_uniqueEdges.size(); p++) { - if (IsAlmostZero(m_uniqueEdges[p] - edge) || + if (IsAlmostZero1(m_uniqueEdges[p] - edge) || IsAlmostZero1(m_uniqueEdges[p] + edge)) { found = true; From 03549ca7c6cef09e5d32ae256f9f8011a0a34173 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 23 Jan 2019 17:24:10 -0800 Subject: [PATCH 06/13] pass force as array instead of value --- .../gym/pybullet_envs/deep_mimic/humanoid.py | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid.py index b2ebf0994..1c7dbcc3a 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid.py @@ -438,18 +438,18 @@ class Humanoid(object): bc.resetJointStateMultiDof(humanoid,leftShoulder, pose._leftShoulderRot) bc.resetJointStateMultiDof(humanoid,leftElbow, pose._leftElbowRot) - bc.setJointMotorControlMultiDof(humanoid,chest,controlMode, targetPosition=pose._chestRot,positionGain=kp, force=200) - bc.setJointMotorControlMultiDof(humanoid,neck,controlMode,targetPosition=pose._neckRot,positionGain=kp, force=50) - bc.setJointMotorControlMultiDof(humanoid,rightHip,controlMode,targetPosition=pose._rightHipRot,positionGain=kp, force=200) - bc.setJointMotorControlMultiDof(humanoid,rightKnee,controlMode,targetPosition=pose._rightKneeRot,positionGain=kp, force=150) - bc.setJointMotorControlMultiDof(humanoid,rightAnkle,controlMode,targetPosition=pose._rightAnkleRot,positionGain=kp, force=90) - bc.setJointMotorControlMultiDof(humanoid,rightShoulder,controlMode,targetPosition=pose._rightShoulderRot,positionGain=kp, force=100) - bc.setJointMotorControlMultiDof(humanoid,rightElbow, controlMode,targetPosition=pose._rightElbowRot,positionGain=kp, force=60) - bc.setJointMotorControlMultiDof(humanoid,leftHip, controlMode,targetPosition=pose._leftHipRot,positionGain=kp, force=200) - bc.setJointMotorControlMultiDof(humanoid,leftKnee, controlMode,targetPosition=pose._leftKneeRot,positionGain=kp, force=150) - bc.setJointMotorControlMultiDof(humanoid,leftAnkle, controlMode,targetPosition=pose._leftAnkleRot,positionGain=kp, force=90) - bc.setJointMotorControlMultiDof(humanoid,leftShoulder, controlMode,targetPosition=pose._leftShoulderRot,positionGain=kp, force=100) - bc.setJointMotorControlMultiDof(humanoid,leftElbow, controlMode,targetPosition=pose._leftElbowRot,positionGain=kp, force=60) + bc.setJointMotorControlMultiDof(humanoid,chest,controlMode, targetPosition=pose._chestRot,positionGain=kp, force=[200]) + bc.setJointMotorControlMultiDof(humanoid,neck,controlMode,targetPosition=pose._neckRot,positionGain=kp, force=[50]) + bc.setJointMotorControlMultiDof(humanoid,rightHip,controlMode,targetPosition=pose._rightHipRot,positionGain=kp, force=[200]) + bc.setJointMotorControlMultiDof(humanoid,rightKnee,controlMode,targetPosition=pose._rightKneeRot,positionGain=kp, force=[150]) + bc.setJointMotorControlMultiDof(humanoid,rightAnkle,controlMode,targetPosition=pose._rightAnkleRot,positionGain=kp, force=[90]) + bc.setJointMotorControlMultiDof(humanoid,rightShoulder,controlMode,targetPosition=pose._rightShoulderRot,positionGain=kp, force=[100]) + bc.setJointMotorControlMultiDof(humanoid,rightElbow, controlMode,targetPosition=pose._rightElbowRot,positionGain=kp, force=[60]) + bc.setJointMotorControlMultiDof(humanoid,leftHip, controlMode,targetPosition=pose._leftHipRot,positionGain=kp, force=[200]) + bc.setJointMotorControlMultiDof(humanoid,leftKnee, controlMode,targetPosition=pose._leftKneeRot,positionGain=kp, force=[150]) + bc.setJointMotorControlMultiDof(humanoid,leftAnkle, controlMode,targetPosition=pose._leftAnkleRot,positionGain=kp, force=[90]) + bc.setJointMotorControlMultiDof(humanoid,leftShoulder, controlMode,targetPosition=pose._leftShoulderRot,positionGain=kp, force=[100]) + bc.setJointMotorControlMultiDof(humanoid,leftElbow, controlMode,targetPosition=pose._leftElbowRot,positionGain=kp, force=[60]) #debug space #if (False): From e637b24237342ddd34aff952f3c362c118b9640a Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 23 Jan 2019 17:35:45 -0800 Subject: [PATCH 07/13] remove Rand.* and (no C++11) --- .../plugins/stablePDPlugin/MathUtil.cpp | 113 -------------- .../plugins/stablePDPlugin/MathUtil.h | 27 +--- .../plugins/stablePDPlugin/Rand.cpp | 140 ------------------ .../plugins/stablePDPlugin/Rand.h | 31 ---- .../CollisionShapes/btConvexPolyhedron.cpp | 2 +- 5 files changed, 4 insertions(+), 309 deletions(-) delete mode 100644 examples/SharedMemory/plugins/stablePDPlugin/Rand.cpp delete mode 100644 examples/SharedMemory/plugins/stablePDPlugin/Rand.h diff --git a/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp b/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp index dcdc07b60..6fcd67e16 100644 --- a/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp +++ b/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp @@ -3,7 +3,6 @@ #define _USE_MATH_DEFINES #include -cRand cMathUtil::gRand = cRand(); int cMathUtil::Clamp(int val, int min, int max) { @@ -45,69 +44,6 @@ double cMathUtil::NormalizeAngle(double theta) return norm_theta; } -double cMathUtil::RandDouble() -{ - return RandDouble(0, 1); -} - -double cMathUtil::RandDouble(double min, double max) -{ - return gRand.RandDouble(min, max); -} - -double cMathUtil::RandDoubleNorm(double mean, double stdev) -{ - return gRand.RandDoubleNorm(mean, stdev); -} - -double cMathUtil::RandDoubleExp(double lambda) -{ - return gRand.RandDoubleExp(lambda); -} - -double cMathUtil::RandDoubleSeed(double seed) -{ - unsigned int int_seed = *reinterpret_cast(&seed); - std::default_random_engine rand_gen(int_seed); - std::uniform_real_distribution dist; - return dist(rand_gen); -} - -int cMathUtil::RandInt() -{ - return gRand.RandInt(); -} - -int cMathUtil::RandInt(int min, int max) -{ - return gRand.RandInt(min, max); -} - -int cMathUtil::RandUint() -{ - return gRand.RandUint(); -} - -int cMathUtil::RandUint(unsigned int min, unsigned int max) -{ - return gRand.RandUint(min, max); -} - -int cMathUtil::RandIntExclude(int min, int max, int exc) -{ - return gRand.RandIntExclude(min, max, exc); -} - -void cMathUtil::SeedRand(unsigned long int seed) -{ - gRand.Seed(seed); - srand(gRand.RandInt()); -} - -int cMathUtil::RandSign() -{ - return gRand.RandSign(); -} double cMathUtil::SmoothStep(double t) { @@ -115,10 +51,6 @@ double cMathUtil::SmoothStep(double t) return val; } -bool cMathUtil::FlipCoin(double p) -{ - return gRand.FlipCoin(p); -} tMatrix cMathUtil::TranslateMat(const tVector& trans) { @@ -665,26 +597,6 @@ double cMathUtil::Sigmoid(double x, double gamma, double bias) return val; } -int cMathUtil::SampleDiscreteProb(const Eigen::VectorXd& probs) -{ - assert(std::abs(probs.sum() - 1) < 0.00001); - double rand = RandDouble(); - - int rand_idx = gInvalidIdx; - int num_probs = static_cast(probs.size()); - for (int i = 0; i < num_probs; ++i) - { - double curr_prob = probs[i]; - rand -= curr_prob; - - if (rand <= 0) - { - rand_idx = i; - break; - } - } - return rand_idx; -} tVector cMathUtil::CalcBarycentric(const tVector& p, const tVector& a, const tVector& b, const tVector& c) { @@ -791,32 +703,7 @@ bool cMathUtil::CheckNextInterval(double delta, double curr_val, double int_size return new_action; } -tVector cMathUtil::SampleRandPt(const tVector& bound_min, const tVector& bound_max) -{ - tVector pt = tVector(RandDouble(bound_min[0], bound_max[0]), - RandDouble(bound_min[1], bound_max[1]), - RandDouble(bound_min[2], bound_max[2]), 0); - return pt; -} -tVector cMathUtil::SampleRandPtBias(const tVector& bound_min, const tVector& bound_max) -{ - return SampleRandPtBias(bound_min, bound_max, 0.5 * (bound_max + bound_min)); -} - -tVector cMathUtil::SampleRandPtBias(const tVector& bound_min, const tVector& bound_max, const tVector& focus) -{ - double t = RandDouble(0, 1); - tVector size = bound_max - bound_min; - tVector new_min = focus + (t * 0.5) * size; - tVector new_max = focus - (t * 0.5) * size; - tVector offset = (bound_min - new_min).cwiseMax(0); - offset += (bound_max - new_max).cwiseMin(0); - new_min += offset; - new_max += offset; - - return SampleRandPt(new_min, new_max); -} void cMathUtil::QuatSwingTwistDecomposition(const tQuaternion& q, const tVector& dir, tQuaternion& out_swing, tQuaternion& out_twist) { diff --git a/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h b/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h index 4e7413ed5..6a2bf695e 100644 --- a/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h +++ b/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h @@ -1,11 +1,10 @@ #pragma once -#include #include "Eigen/Dense" #include "Eigen/StdVector" #include "Eigen/Geometry" -#include "Rand.h" + #define _USE_MATH_DEFINES #include "math.h" @@ -47,20 +46,6 @@ public: static double NormalizeAngle(double theta); - // rand number - static double RandDouble(); - static double RandDouble(double min, double max); - static double RandDoubleNorm(double mean, double stdev); - static double RandDoubleExp(double lambda); - static double RandDoubleSeed(double seed); - static int RandInt(); - static int RandInt(int min, int max); - static int RandUint(); - static int RandUint(unsigned int min, unsigned int max); - static int RandIntExclude(int min, int max, int exc); - static void SeedRand(unsigned long int seed); - static int RandSign(); - static bool FlipCoin(double p = 0.5); static double SmoothStep(double t); // matrices @@ -117,7 +102,6 @@ public: static double Sigmoid(double x); static double Sigmoid(double x, double gamma, double bias); - static int SampleDiscreteProb(const Eigen::VectorXd& probs); static tVector CalcBarycentric(const tVector& p, const tVector& a, const tVector& b, const tVector& c); static bool ContainsAABB(const tVector& pt, const tVector& aabb_min, const tVector& aabb_max); @@ -134,19 +118,14 @@ public: // check if curr_val and curr_val - delta belong to different intervals static bool CheckNextInterval(double delta, double curr_val, double int_size); - static tVector SampleRandPt(const tVector& bound_min, const tVector& bound_max); - // samples a bound within the given bounds with a benter towards the focus pt - static tVector SampleRandPtBias(const tVector& bound_min, const tVector& bound_max); - static tVector SampleRandPtBias(const tVector& bound_min, const tVector& bound_max, const tVector& focus); - + static void QuatSwingTwistDecomposition(const tQuaternion& q, const tVector& dir, tQuaternion& out_swing, tQuaternion& out_twist); static tQuaternion ProjectQuat(const tQuaternion& q, const tVector& dir); static void ButterworthFilter(double dt, double cutoff, Eigen::VectorXd& out_x); private: - static cRand gRand; - + template static T SignAux(T val) { diff --git a/examples/SharedMemory/plugins/stablePDPlugin/Rand.cpp b/examples/SharedMemory/plugins/stablePDPlugin/Rand.cpp deleted file mode 100644 index 599d2d0b7..000000000 --- a/examples/SharedMemory/plugins/stablePDPlugin/Rand.cpp +++ /dev/null @@ -1,140 +0,0 @@ -#include "Rand.h" -#include -#include -#include - -cRand::cRand() -{ - unsigned long int seed = static_cast(time(NULL)); - mRandGen = std::default_random_engine(seed); - mRandDoubleDist = std::uniform_real_distribution(0, 1); - mRandDoubleDistNorm = std::normal_distribution(0, 1); - mRandIntDist = std::uniform_int_distribution(std::numeric_limits::min() + 1, std::numeric_limits::max()); // + 1 since there is one more neg int than pos int - mRandUintDist = std::uniform_int_distribution(std::numeric_limits::min(), std::numeric_limits::max()); -} - -cRand::cRand(unsigned long int seed) -{ - Seed(seed); -} - -cRand::~cRand() -{ -} - -double cRand::RandDouble() -{ - return mRandDoubleDist(mRandGen); -} - -double cRand::RandDouble(double min, double max) -{ - if (min == max) - { - return min; - } - - // generate random double in [min, max] - double rand_double = mRandDoubleDist(mRandGen); - rand_double = min + (rand_double * (max - min)); - return rand_double; -} - -double cRand::RandDoubleExp(double lambda) -{ - std::exponential_distribution dist(lambda); - double rand_double = dist(mRandGen); - return rand_double; -} - -double cRand::RandDoubleNorm(double mean, double stdev) -{ - double rand_double = mRandDoubleDistNorm(mRandGen); - rand_double = mean + stdev * rand_double; - return rand_double; -} - -int cRand::RandInt() -{ - return mRandIntDist(mRandGen); -} - -int cRand::RandInt(int min, int max) -{ - if (min == max) - { - return min; - } - - // generate random double in [min, max) - int delta = max - min; - int rand_int = std::abs(RandInt()); - rand_int = min + rand_int % delta; - - return rand_int; -} - -int cRand::RandUint() -{ - return mRandUintDist(mRandGen); -} - -int cRand::RandUint(unsigned int min, unsigned int max) -{ - if (min == max) - { - return min; - } - - // generate random double in [min, max) - int delta = max - min; - int rand_int = RandUint(); - rand_int = min + rand_int % delta; - - return rand_int; -} - -int cRand::RandIntExclude(int min, int max, int exc) -{ - int rand_int = 0; - if (exc < min || exc >= max) - { - rand_int = RandInt(min, max); - } - else - { - int new_max = max - 1; - if (new_max <= min) - { - rand_int = min; - } - else - { - rand_int = RandInt(min, new_max); - if (rand_int >= exc) - { - ++rand_int; - } - } - } - return rand_int; -} - -void cRand::Seed(unsigned long int seed) -{ - mRandGen.seed(seed); - mRandDoubleDist.reset(); - mRandDoubleDistNorm.reset(); - mRandIntDist.reset(); - mRandUintDist.reset(); -} - -int cRand::RandSign() -{ - return FlipCoin() ? -1 : 1; -} - -bool cRand::FlipCoin(double p) -{ - return (RandDouble(0, 1) < p); -} diff --git a/examples/SharedMemory/plugins/stablePDPlugin/Rand.h b/examples/SharedMemory/plugins/stablePDPlugin/Rand.h deleted file mode 100644 index c7dd543bf..000000000 --- a/examples/SharedMemory/plugins/stablePDPlugin/Rand.h +++ /dev/null @@ -1,31 +0,0 @@ -#pragma once - -#include - -class cRand -{ -public: - cRand(); - cRand(unsigned long int seed); - virtual ~cRand(); - - virtual double RandDouble(); - virtual double RandDouble(double min, double max); - virtual double RandDoubleExp(double lambda); - virtual double RandDoubleNorm(double mean, double stdev); - virtual int RandInt(); - virtual int RandInt(int min, int max); - virtual int RandUint(); - virtual int RandUint(unsigned int min, unsigned int max); - virtual int RandIntExclude(int min, int max, int exc); - virtual void Seed(unsigned long int seed); - virtual int RandSign(); - virtual bool FlipCoin(double p = 0.5); - -private: - std::default_random_engine mRandGen; - std::uniform_real_distribution mRandDoubleDist; - std::normal_distribution mRandDoubleDistNorm; - std::uniform_int_distribution mRandIntDist; - std::uniform_int_distribution mRandUintDist; -}; \ No newline at end of file diff --git a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp index d5ec6cb92..9694f4ddb 100644 --- a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp +++ b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp @@ -122,7 +122,7 @@ void btConvexPolyhedron::initialize() for (int p = 0; p < m_uniqueEdges.size(); p++) { - if (IsAlmostZero(m_uniqueEdges[p] - edge) || + if (IsAlmostZero1(m_uniqueEdges[p] - edge) || IsAlmostZero1(m_uniqueEdges[p] + edge)) { found = true; From dbf93b7129001c0ac6418d15975de6c00aad8de3 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 23 Jan 2019 17:36:30 -0800 Subject: [PATCH 08/13] remove Rand.cpp from setup.py --- setup.py | 1 - 1 file changed, 1 deletion(-) diff --git a/setup.py b/setup.py index 9d6bd9033..f115d8037 100644 --- a/setup.py +++ b/setup.py @@ -105,7 +105,6 @@ sources = ["examples/pybullet/pybullet.c"]\ +["examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp"]\ +["examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp"]\ +["examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp"]\ -+["examples/SharedMemory/plugins/stablePDPlugin/Rand.cpp"]\ +["examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp"]\ +["examples/SharedMemory/plugins/stablePDPlugin/SpAlg.cpp"]\ +["examples/SharedMemory/PhysicsClientUDP.cpp"]\ From 899e3274e88aa6bcd295b1fa7668ee69ecf4760b Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 23 Jan 2019 17:44:49 -0800 Subject: [PATCH 09/13] fix more compile issues --- examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h b/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h index 6a2bf695e..8d3fe27c7 100644 --- a/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h +++ b/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h @@ -17,9 +17,6 @@ typedef Eigen::Matrix4d tMatrix; typedef Eigen::Matrix3d tMatrix3; typedef Eigen::Quaterniond tQuaternion; -template -using tEigenArr = std::vector >; -typedef tEigenArr tVectorArr; const double gRadiansToDegrees = 57.2957795; const double gDegreesToRadians = 1.0 / gRadiansToDegrees; From 587500dc16abdf055ca65cfd2562b61467a5df74 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 23 Jan 2019 18:23:09 -0800 Subject: [PATCH 10/13] remove auto (no C++11) --- .../plugins/stablePDPlugin/KinTree.cpp | 4 ++-- .../plugins/stablePDPlugin/RBDUtil.cpp | 21 ++++++++++--------- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp b/examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp index 1b326f02d..e674abb0b 100644 --- a/examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp +++ b/examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp @@ -1793,8 +1793,8 @@ void cKinTree::MirrorPoseStance(const Eigen::MatrixXd& joint_mat, const std::vec { int param_offset = cKinTree::GetParamOffset(joint_mat, j); int param_size = cKinTree::GetParamSize(joint_mat, j); - auto joint_params = out_pose.segment(param_offset, param_size); - + Eigen::VectorBlock& joint_params = out_pose.segment(param_offset, param_size); + if (joint_type == cKinTree::eJointTypeRevolute || joint_type == cKinTree::eJointTypeFixed) { diff --git a/examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp b/examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp index 64a5820a8..864c17135 100644 --- a/examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp +++ b/examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp @@ -31,7 +31,8 @@ void cRBDUtil::SolveInvDyna(const cRBDModel& model, const Eigen::VectorXd& acc, cSpAlg::tSpTrans parent_child_trans = model.GetSpParentChildTrans(j); cSpAlg::tSpTrans world_child_trans = model.GetSpWorldJointTrans(j); - const auto S = model.GetJointSubspace(j); + const Eigen::Block S = model.GetJointSubspace(j); + Eigen::VectorXd q; Eigen::VectorXd dq; Eigen::VectorXd ddq; @@ -84,7 +85,7 @@ void cRBDUtil::SolveInvDyna(const cRBDModel& model, const Eigen::VectorXd& acc, if (cKinTree::IsValidBody(body_defs, j)) { cSpAlg::tSpVec curr_f = fs.row(j); - const auto S = model.GetJointSubspace(j); + const const Eigen::Block S = model.GetJointSubspace(j); Eigen::VectorXd curr_tau = S.transpose() * curr_f; cKinTree::SetJointParams(joint_mat, j, curr_tau, out_tau); @@ -156,7 +157,7 @@ void cRBDUtil::BuildMassMat(const cRBDModel& model, Eigen::MatrixXd& inertia_buf { if (cKinTree::IsValidBody(body_defs, j)) { - const auto curr_I = Is.block(j * svs, 0, svs, svs); + Eigen::Block< Eigen::MatrixXd, -1, -1, false> curr_I = Is.block(j * svs, 0, svs, svs); int parent_id = cKinTree::GetParent(joint_mat, j); if (cKinTree::HasParent(joint_mat, j)) { @@ -166,7 +167,7 @@ void cRBDUtil::BuildMassMat(const cRBDModel& model, Eigen::MatrixXd& inertia_buf Is.block(parent_id * svs, 0, svs, svs) += child_parent_mat * curr_I * parent_child_mat; } - const auto S = model.GetJointSubspace(j); + const Eigen::Block S = model.GetJointSubspace(j); int dim = cKinTree::GetParamSize(joint_mat, j); if (dim > 0) { @@ -186,7 +187,7 @@ void cRBDUtil::BuildMassMat(const cRBDModel& model, Eigen::MatrixXd& inertia_buf if (curr_dim > 0) { - const auto curr_S = model.GetJointSubspace(curr_id); + const Eigen::Block curr_S = model.GetJointSubspace(curr_id); H.block(offset, curr_offset, dim, curr_dim) = F.transpose() * curr_S; H.block(curr_offset, offset, curr_dim, dim) = H.block(offset, curr_offset, dim, curr_dim).transpose(); } @@ -261,7 +262,7 @@ Eigen::MatrixXd cRBDUtil::MultJacobianEndEff(const Eigen::MatrixXd& joint_mat, c int size = cKinTree::GetParamSize(joint_mat, curr_id); Eigen::VectorXd curr_q; cKinTree::GetJointParams(joint_mat, q, curr_id, curr_q); - const auto curr_J = J.block(0, offset, cSpAlg::gSpVecSize, size); + const Eigen::Block curr_J = J.block(0, offset, cSpAlg::gSpVecSize, size); joint_vel += curr_J * curr_q; curr_id = cKinTree::GetParent(joint_mat, curr_id); @@ -299,7 +300,7 @@ Eigen::MatrixXd cRBDUtil::ExtractEndEffJacobian(const Eigen::MatrixXd& joint_mat { int offset = cKinTree::GetParamOffset(joint_mat, curr_id); int size = cKinTree::GetParamSize(joint_mat, curr_id); - const auto curr_J = J.block(0, offset, cSpAlg::gSpVecSize, size); + const Eigen::Block curr_J = J.block(0, offset, cSpAlg::gSpVecSize, size); J_end_eff.block(0, offset, cSpAlg::gSpVecSize, size) = curr_J; curr_id = cKinTree::GetParent(joint_mat, curr_id); @@ -353,7 +354,7 @@ void cRBDUtil::BuildCOMJacobian(const cRBDModel& model, const Eigen::MatrixXd& J int size = cKinTree::GetParamSize(joint_mat, curr_id); const Eigen::MatrixXd& J_block = J.block(0, offset, cSpAlg::gSpVecSize, size); - auto J_com_block = out_J.block(0, offset, cSpAlg::gSpVecSize, size); + Eigen::Block J_com_block = out_J.block(0, offset, cSpAlg::gSpVecSize, size); J_com_block += mass_frac * cSpAlg::ApplyTransM(body_pos_trans, J_block); curr_id = cKinTree::GetParent(joint_mat, curr_id); @@ -456,7 +457,7 @@ cSpAlg::tSpVec cRBDUtil::CalcVelProdAcc(const cRBDModel& model, const Eigen::Mat Eigen::VectorXd dq; cKinTree::GetJointParams(joint_mat, pose, curr_id, q); cKinTree::GetJointParams(joint_mat, vel, curr_id, dq); - const auto curr_Jd = Jd.block(0, offset, cSpAlg::gSpVecSize, size); + const Eigen::Block curr_Jd = Jd.block(0, offset, cSpAlg::gSpVecSize, size); cSpAlg::tSpVec cj = BuildCj(joint_mat, q, dq, curr_id); if (cj.squaredNorm() > 0) @@ -1035,7 +1036,7 @@ void cRBDUtil::CalcGravityForce(const cRBDModel& model, Eigen::VectorXd& out_g_f if (cKinTree::IsValidBody(body_defs, j)) { cSpAlg::tSpVec curr_f = fs.row(j); - const auto S = model.GetJointSubspace(j); + const Eigen::Block S = model.GetJointSubspace(j); Eigen::VectorXd curr_tau = S.transpose() * curr_f; cKinTree::SetJointParams(joint_mat, j, curr_tau, out_g_force); From 66f4b276ec406670306b3bb45352563a18f74455 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 23 Jan 2019 19:25:12 -0800 Subject: [PATCH 11/13] fix compile issue --- examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp b/examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp index e674abb0b..621fa2eec 100644 --- a/examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp +++ b/examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp @@ -1793,7 +1793,7 @@ void cKinTree::MirrorPoseStance(const Eigen::MatrixXd& joint_mat, const std::vec { int param_offset = cKinTree::GetParamOffset(joint_mat, j); int param_size = cKinTree::GetParamSize(joint_mat, j); - Eigen::VectorBlock& joint_params = out_pose.segment(param_offset, param_size); + Eigen::VectorBlock joint_params = out_pose.segment(param_offset, param_size); if (joint_type == cKinTree::eJointTypeRevolute || joint_type == cKinTree::eJointTypeFixed) From 43d3cdfa38780da92be995624523899fd6ac8285 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 23 Jan 2019 19:34:21 -0800 Subject: [PATCH 12/13] make Travis CI pass --- .../plugins/stablePDPlugin/KinTree.cpp | 58 ------------------- .../plugins/stablePDPlugin/KinTree.h | 3 +- .../plugins/stablePDPlugin/Shape.cpp | 1 + 3 files changed, 2 insertions(+), 60 deletions(-) diff --git a/examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp b/examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp index 621fa2eec..2aab56904 100644 --- a/examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp +++ b/examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp @@ -1771,64 +1771,6 @@ void cKinTree::NormalizePoseHeading(const Eigen::MatrixXd& joint_mat, Eigen::Vec } } -void cKinTree::MirrorPoseStance(const Eigen::MatrixXd& joint_mat, const std::vector mirror_joints0, const std::vector mirror_joints1, Eigen::VectorXd& out_pose) -{ - // mirrors along xy plane - assert(mirror_joints0.size() == mirror_joints1.size()); - int num_joints = cKinTree::GetNumJoints(joint_mat); - - tQuaternion root_rot = cKinTree::GetRootRot(joint_mat, out_pose); - tVector root_pos = cKinTree::GetRootPos(joint_mat, out_pose); - root_pos[2] *= -1; - root_rot = cMathUtil::MirrorQuaternion(root_rot, cMathUtil::eAxisZ); - - cKinTree::SetRootRot(joint_mat, root_rot, out_pose); - cKinTree::SetRootPos(joint_mat, root_pos, out_pose); - - for (int j = 0; j < num_joints; ++j) - { - cKinTree::eJointType joint_type = cKinTree::GetJointType(joint_mat, j); - bool is_root = cKinTree::IsRoot(joint_mat, j); - if (!is_root) - { - int param_offset = cKinTree::GetParamOffset(joint_mat, j); - int param_size = cKinTree::GetParamSize(joint_mat, j); - Eigen::VectorBlock joint_params = out_pose.segment(param_offset, param_size); - - if (joint_type == cKinTree::eJointTypeRevolute - || joint_type == cKinTree::eJointTypeFixed) - { - } - else if (joint_type == cKinTree::eJointTypeSpherical) - { - tQuaternion quat = cMathUtil::VecToQuat(joint_params); - quat = cMathUtil::MirrorQuaternion(quat, cMathUtil::eAxisZ); - joint_params = cMathUtil::QuatToVec(quat); - } - else - { - assert(false); // unsupported joint type - } - } - } - - for (size_t i = 0; i < mirror_joints0.size(); ++i) - { - int id0 = mirror_joints0[i]; - int id1 = mirror_joints1[i]; - - int offset0 = cKinTree::GetParamOffset(joint_mat, id0); - int size0 = cKinTree::GetParamSize(joint_mat, id0); - int offset1 = cKinTree::GetParamOffset(joint_mat, id1); - int size1 = cKinTree::GetParamSize(joint_mat, id1); - - Eigen::VectorXd params0 = out_pose.segment(offset0, size0); - Eigen::VectorXd params1 = out_pose.segment(offset1, size1); - out_pose.segment(offset0, size0) = params1; - out_pose.segment(offset1, size1) = params0; - } -} - tMatrix cKinTree::ChildParentTransRoot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id) { diff --git a/examples/SharedMemory/plugins/stablePDPlugin/KinTree.h b/examples/SharedMemory/plugins/stablePDPlugin/KinTree.h index 6c2fcc31e..a1c459f43 100644 --- a/examples/SharedMemory/plugins/stablePDPlugin/KinTree.h +++ b/examples/SharedMemory/plugins/stablePDPlugin/KinTree.h @@ -238,8 +238,7 @@ public: static void NormalizePoseHeading(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose); static void NormalizePoseHeading(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose, Eigen::VectorXd& out_vel); - static void MirrorPoseStance(const Eigen::MatrixXd& joint_mat, const std::vector mirror_joints0, const std::vector mirror_joints1, Eigen::VectorXd& out_pose); - + protected: static bool ParseJointType(const std::string& type_str, eJointType& out_joint_type); diff --git a/examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp b/examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp index 67fae6ca0..a8507257b 100644 --- a/examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp +++ b/examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp @@ -1,5 +1,6 @@ #include "Shape.h" #include +#include bool cShape::ParseShape(const std::string& str, eShape& out_shape) { From 7df6adb9f845d56ef568c218d8597ed89daef8d1 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 23 Jan 2019 19:58:19 -0800 Subject: [PATCH 13/13] fix compilation, make travis CI happy --- examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp b/examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp index 864c17135..6e885dea4 100644 --- a/examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp +++ b/examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp @@ -85,7 +85,7 @@ void cRBDUtil::SolveInvDyna(const cRBDModel& model, const Eigen::VectorXd& acc, if (cKinTree::IsValidBody(body_defs, j)) { cSpAlg::tSpVec curr_f = fs.row(j); - const const Eigen::Block S = model.GetJointSubspace(j); + const Eigen::Block S = model.GetJointSubspace(j); Eigen::VectorXd curr_tau = S.transpose() * curr_f; cKinTree::SetJointParams(joint_mat, j, curr_tau, out_tau); @@ -1048,4 +1048,4 @@ void cRBDUtil::CalcGravityForce(const cRBDModel& model, Eigen::VectorXd& out_g_f } } } -} \ No newline at end of file +}