From ecd814c9c5360ae394c23c81ae0dd51d30cf32e9 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 2 Sep 2016 16:40:56 -0700 Subject: [PATCH 01/14] export contact friction/damping through URDF and API convert from contact friction/damping to cfm/erp btCollisionObject::setContactFrictionAndDamping --- data/cube_no_friction.urdf | 4 +- data/cube_small.urdf | 4 +- data/cube_soft.urdf | 32 +++++++ data/multibody.bullet | Bin 14584 -> 14660 bytes .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 5 +- .../Importers/ImportURDFDemo/URDFJointTypes.h | 10 +- .../Importers/ImportURDFDemo/UrdfParser.cpp | 77 ++++++++++++--- examples/MultiBody/MultiBodySoftContact.cpp | 21 +--- examples/RigidBody/RigidBodySoftContact.cpp | 20 ++-- .../RoboticsLearning/GripperGraspExample.cpp | 2 +- .../RoboticsLearning/R2D2GraspExample.cpp | 2 +- .../CollisionDispatch/btCollisionObject.cpp | 4 + .../CollisionDispatch/btCollisionObject.h | 38 +++++++- .../CollisionDispatch/btManifoldResult.cpp | 27 +++++- .../CollisionDispatch/btManifoldResult.h | 3 + .../NarrowPhaseCollision/btManifoldPoint.h | 15 ++- .../btSequentialImpulseConstraintSolver.cpp | 38 +++++++- .../Featherstone/btMultiBodyConstraint.cpp | 2 +- .../btMultiBodyConstraintSolver.cpp | 90 ++++++------------ 19 files changed, 269 insertions(+), 125 deletions(-) create mode 100644 data/cube_soft.urdf diff --git a/data/cube_no_friction.urdf b/data/cube_no_friction.urdf index aef2bfa15..6df23a865 100644 --- a/data/cube_no_friction.urdf +++ b/data/cube_no_friction.urdf @@ -4,8 +4,8 @@ - - + + diff --git a/data/cube_small.urdf b/data/cube_small.urdf index 23ae7fa3b..f6e271ee3 100644 --- a/data/cube_small.urdf +++ b/data/cube_small.urdf @@ -3,10 +3,8 @@ - + - - diff --git a/data/cube_soft.urdf b/data/cube_soft.urdf new file mode 100644 index 000000000..af40dd379 --- /dev/null +++ b/data/cube_soft.urdf @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/multibody.bullet b/data/multibody.bullet index 31800308cedc7804a8642a0343c167373049f94b..50ed04b49b35aa23c1e25653a29892410d61977c 100644 GIT binary patch delta 1400 zcmai!ZAep57{{M8ue;ajO3G;Jh;r)6Z05k1f^D^|C|34jsc5fQre$iHmT4}EUZ99| zJ_JQz5Tz7J7!<|q1u4iGRv#jWsEDXONkN)K_B`IlGz>cM=efW0Kj;6P9q!#gb=RIK z?>e7vao+YhbkqW5wEL@*)9%*NiLi?8S05Vl|_t zqY_cHLFFp2j*LrH!A^Q6e>PS-KPPjgc?W>wPvbB)GO#A_^CYaZP;sc~lrGZ^_GMV% z&gHGc-R2d3+d}+=`Px-)8Sx9|g(tcOh(9u?W~{zTJcumhJ0)JG8Z_nhv0r6@`xbfM zU~O)?q9;#DvsKc{Gt8M0bxFk6nLh_geZ<|&d66?h#P6BQzjePO9%Qa~|K$|%O*n3ooRe@;6-F~6wUah_Od(~s0Yl-5t|WG?)1#DjNamDj?w2~9k?#+y?J2I*A3 zYCWNl3l>&HYm+5h@6@K%6ho%I!#(_3;VGGyFhiE^uNd)}*Nynuy1!w>XD&e=-}|H%6XC%Z zTkroYqNX_4aVtm(PapIJAGasDtLZzzVTq6JA8H=0=)1_*U4VhUD0h7*6x^2o67Lt_ zTz-D=aC=hcIp*3-N9RBLnbI3tcoH*DqC34YJc_`tuEJ{d@N~zdXwWXw_ zYt6w*e)hODQX9*3{{TWDKyw66sSPu zp?0GxajptgjoO2%LDk|R_M+-UI@Dv`CyF5kCSs-;GtHQ3#!NG2nlY1d)G@hgB?hV( zsA3>BP%%^;6YAv94G@8``|)56*z^E4Y7|9KfK8i*n<}sgYZ7b(`j2bGLlg)ndXsUF L8TY8Tht~WCNjzzP delta 1335 zcmb`GOH30%7{_N|%NAi3XoZ%yZJ~g)3SvQYODSNY5Pb0rh;L9t<0y)5F7wS;2NypadKOrQk3)0%YI@W#Hp` zuluEKOu_AzD-ssT%n|luLwLB$jRCcy&*fCN?`RD05b~xWI6ikdd0_bR`)gF6+^v(V z{eEQy>y*U{X7*Ijvz3VCam=Vms8CQLhrGo#FQ2lF$se?JxB^$;YT~=ji}F3Jbva+D zCx9z(H(Wj8GW-G78#Y{qyWwj6_X4B4ydh97*J=frSj2<^R}1DH#p=QSI)Z_4bz``? zt{qTese*~kMrLN&Xtgh4)!_SL@~D5D>fY+SDg4<}(xbHgoWC$9gjCtM2qA672fX%6?aRrG(x zmr^!uk$ik7?i6Fi3GeYZ_Cy?LgHTd>cQSFEkk}setRollingFriction(contactInfo.m_rollingFriction); } - + if ((contactInfo.m_flags & URDF_CONTACT_HAS_STIFFNESS_DAMPING)!=0) + { + col->setContactStiffnessAndDamping(contactInfo.m_contactStiffness,contactInfo.m_contactDamping); + } if (mbLinkIndex>=0) //???? double-check +/- 1 { diff --git a/examples/Importers/ImportURDFDemo/URDFJointTypes.h b/examples/Importers/ImportURDFDemo/URDFJointTypes.h index 2a6bdfef4..08c984268 100644 --- a/examples/Importers/ImportURDFDemo/URDFJointTypes.h +++ b/examples/Importers/ImportURDFDemo/URDFJointTypes.h @@ -19,7 +19,8 @@ enum URDF_LinkContactFlags URDF_CONTACT_HAS_ROLLING_FRICTION=2, URDF_CONTACT_HAS_INERTIA_SCALING=2, URDF_CONTACT_HAS_CONTACT_CFM=4, - URDF_CONTACT_HAS_CONTACT_ERP=8 + URDF_CONTACT_HAS_CONTACT_ERP=8, + URDF_CONTACT_HAS_STIFFNESS_DAMPING=16, }; struct URDFLinkContactInfo @@ -29,6 +30,9 @@ struct URDFLinkContactInfo btScalar m_inertiaScaling; btScalar m_contactCfm; btScalar m_contactErp; + btScalar m_contactStiffness; + btScalar m_contactDamping; + int m_flags; URDFLinkContactInfo() @@ -36,7 +40,9 @@ struct URDFLinkContactInfo m_rollingFriction(0), m_inertiaScaling(1), m_contactCfm(0), - m_contactErp(0) + m_contactErp(0), + m_contactStiffness(1e4), + m_contactDamping(1) { m_flags = URDF_CONTACT_HAS_LATERAL_FRICTION; } diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index bb0af5f79..3fee80465 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -647,26 +647,75 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi } } - TiXmlElement *rolling_xml = ci->FirstChildElement("rolling_friction"); - if (rolling_xml) { - if (m_parseSDF) + TiXmlElement *rolling_xml = ci->FirstChildElement("rolling_friction"); + if (rolling_xml) { - link.m_contactInfo.m_rollingFriction = urdfLexicalCast(rolling_xml->GetText()); - link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION; - } else - { - if (!rolling_xml->Attribute("value")) + if (m_parseSDF) { - logger->reportError("Link/contact: rolling friction element must have value attribute"); - return false; + link.m_contactInfo.m_rollingFriction = urdfLexicalCast(rolling_xml->GetText()); + link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION; + } else + { + if (!rolling_xml->Attribute("value")) + { + logger->reportError("Link/contact: rolling friction element must have value attribute"); + return false; + } + + link.m_contactInfo.m_rollingFriction = urdfLexicalCast(rolling_xml->Attribute("value")); + link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION; + } - - link.m_contactInfo.m_rollingFriction = urdfLexicalCast(rolling_xml->Attribute("value")); - link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION; - } } + + { + + TiXmlElement *stiffness_xml = ci->FirstChildElement("stiffness"); + if (stiffness_xml) + { + if (m_parseSDF) + { + link.m_contactInfo.m_contactStiffness = urdfLexicalCast(stiffness_xml->GetText()); + link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING; + } else + { + if (!stiffness_xml->Attribute("value")) + { + logger->reportError("Link/contact: stiffness element must have value attribute"); + return false; + } + + link.m_contactInfo.m_contactStiffness = urdfLexicalCast(stiffness_xml->Attribute("value")); + link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING; + + } + } + } + { + + TiXmlElement *damping_xml = ci->FirstChildElement("damping"); + if (damping_xml) + { + if (m_parseSDF) + { + link.m_contactInfo.m_contactDamping = urdfLexicalCast(damping_xml->GetText()); + link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING; + } else + { + if (!damping_xml->Attribute("value")) + { + logger->reportError("Link/contact: damping element must have value attribute"); + return false; + } + + link.m_contactInfo.m_contactDamping = urdfLexicalCast(damping_xml->Attribute("value")); + link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING; + + } + } + } } } diff --git a/examples/MultiBody/MultiBodySoftContact.cpp b/examples/MultiBody/MultiBodySoftContact.cpp index cc5d1ff9a..b7ce1c9f6 100644 --- a/examples/MultiBody/MultiBodySoftContact.cpp +++ b/examples/MultiBody/MultiBodySoftContact.cpp @@ -36,7 +36,6 @@ public: }; -extern ContactAddedCallback gContactAddedCallback; MultiBodySoftContact::MultiBodySoftContact(struct GUIHelperInterface* helper) @@ -47,18 +46,7 @@ m_once(true) MultiBodySoftContact::~MultiBodySoftContact() { - gContactAddedCallback = 0; -} - - -static bool btMultiBodySoftContactCallback(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1) -{ - cp.m_contactCFM = 0.3; - cp.m_contactERP = 0.2; - cp.m_contactPointFlags |= BT_CONTACT_FLAG_HAS_CONTACT_CFM; - cp.m_contactPointFlags |= BT_CONTACT_FLAG_HAS_CONTACT_ERP; - return true; - + } @@ -66,7 +54,7 @@ static bool btMultiBodySoftContactCallback(btManifoldPoint& cp, const btCollisio void MultiBodySoftContact::initPhysics() { - gContactAddedCallback = btMultiBodySoftContactCallback; + int upAxis = 2; m_guiHelper->setUpAxis(upAxis); @@ -109,12 +97,13 @@ void MultiBodySoftContact::initPhysics() start.setOrigin(groundOrigin); // start.setRotation(groundOrn); btRigidBody* body = createRigidBody(0,start,box); + + //setContactStiffnessAndDamping will enable compliant rigid body contact + body->setContactStiffnessAndDamping(300,10); btVector4 color = colors[curColor]; curColor++; curColor&=3; m_guiHelper->createRigidBodyGraphicsObject(body,color); - int flags = body->getCollisionFlags(); - body->setCollisionFlags(flags|btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); } diff --git a/examples/RigidBody/RigidBodySoftContact.cpp b/examples/RigidBody/RigidBodySoftContact.cpp index 1ea1bb002..1e7cd2c54 100644 --- a/examples/RigidBody/RigidBodySoftContact.cpp +++ b/examples/RigidBody/RigidBodySoftContact.cpp @@ -29,7 +29,7 @@ subject to the following restrictions: #include "BulletDynamics/MLCPSolvers/btMLCPSolver.h" #include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" -extern ContactAddedCallback gContactAddedCallback; + struct RigidBodySoftContact : public CommonRigidBodyBase @@ -40,7 +40,7 @@ struct RigidBodySoftContact : public CommonRigidBodyBase } virtual ~RigidBodySoftContact() { - gContactAddedCallback = 0; + } virtual void initPhysics(); virtual void renderScene(); @@ -55,21 +55,12 @@ struct RigidBodySoftContact : public CommonRigidBodyBase }; -static bool btRigidBodySoftContactCallback(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1) -{ - cp.m_contactCFM = 0.3; - cp.m_contactERP = 0.2; - cp.m_contactPointFlags |= BT_CONTACT_FLAG_HAS_CONTACT_CFM; - cp.m_contactPointFlags |= BT_CONTACT_FLAG_HAS_CONTACT_ERP; - return true; - -} void RigidBodySoftContact::initPhysics() { - gContactAddedCallback = btRigidBodySoftContactCallback; + m_guiHelper->setUpAxis(1); @@ -120,8 +111,9 @@ void RigidBodySoftContact::initPhysics() { btScalar mass(0.); btRigidBody* body = createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1)); - int flags = body->getCollisionFlags(); - body->setCollisionFlags(flags|btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); + + body->setContactStiffnessAndDamping(300,10); + } diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 434454985..9a7ab8e7b 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -153,7 +153,7 @@ public: } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); - //m_robotSim.setNumSimulationSubSteps(4); + m_robotSim.setNumSimulationSubSteps(4); } if ((m_options & eTWO_POINT_GRASP)!=0) diff --git a/examples/RoboticsLearning/R2D2GraspExample.cpp b/examples/RoboticsLearning/R2D2GraspExample.cpp index e4a412d73..e0c9cadd1 100644 --- a/examples/RoboticsLearning/R2D2GraspExample.cpp +++ b/examples/RoboticsLearning/R2D2GraspExample.cpp @@ -117,7 +117,7 @@ public: b3RobotSimLoadFileArgs args(""); b3RobotSimLoadFileResults results; { - args.m_fileName = "cube.urdf"; + args.m_fileName = "cube_soft.urdf"; args.m_startPosition.setValue(0,0,2.5); args.m_startOrientation.setEulerZYX(0,0.2,0); m_robotSim.loadFile(args,results); diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index 032a5828d..e9c889b2c 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -33,6 +33,8 @@ btCollisionObject::btCollisionObject() m_friction(btScalar(0.5)), m_restitution(btScalar(0.)), m_rollingFriction(0.0f), + m_contactDamping(.1), + m_contactStiffness(1e4), m_internalType(CO_COLLISION_OBJECT), m_userObjectPointer(0), m_userIndex2(-1), @@ -92,6 +94,8 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali dataOut->m_deactivationTime = m_deactivationTime; dataOut->m_friction = m_friction; dataOut->m_rollingFriction = m_rollingFriction; + dataOut->m_contactDamping = m_contactDamping; + dataOut->m_contactStiffness = m_contactStiffness; dataOut->m_restitution = m_restitution; dataOut->m_internalType = m_internalType; diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/src/BulletCollision/CollisionDispatch/btCollisionObject.h index a96809f91..5e08609e3 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -86,6 +86,10 @@ protected: btScalar m_friction; btScalar m_restitution; btScalar m_rollingFriction; + btScalar m_contactDamping; + btScalar m_contactStiffness; + + ///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc. ///do not assign your own m_internalType unless you write a new dynamics object class. @@ -129,7 +133,8 @@ public: CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution) CF_CHARACTER_OBJECT = 16, CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing - CF_DISABLE_SPU_COLLISION_PROCESSING = 64//disable parallel/SPU processing + CF_DISABLE_SPU_COLLISION_PROCESSING = 64,//disable parallel/SPU processing + CF_HAS_CONTACT_STIFFNESS_DAMPING = 128 }; enum CollisionObjectTypes @@ -319,7 +324,31 @@ public: return m_rollingFriction; } - + void setContactStiffnessAndDamping(btScalar stiffness, btScalar damping) + { + m_updateRevision++; + m_contactStiffness = stiffness; + m_contactDamping = damping; + + m_collisionFlags |=CF_HAS_CONTACT_STIFFNESS_DAMPING; + + //avoid divisions by zero... + if (m_contactStiffness< SIMD_EPSILON) + { + m_contactStiffness = SIMD_EPSILON; + } + } + + btScalar getContactStiffness() const + { + return m_contactStiffness; + } + + btScalar getContactDamping() const + { + return m_contactDamping; + } + ///reserved for Bullet internal usage int getInternalType() const { @@ -541,6 +570,8 @@ struct btCollisionObjectDoubleData double m_deactivationTime; double m_friction; double m_rollingFriction; + double m_contactDamping; + double m_contactStiffness; double m_restitution; double m_hitFraction; double m_ccdSweptSphereRadius; @@ -574,7 +605,8 @@ struct btCollisionObjectFloatData float m_deactivationTime; float m_friction; float m_rollingFriction; - + float m_contactDamping; + float m_contactStiffness; float m_restitution; float m_hitFraction; float m_ccdSweptSphereRadius; diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index 4b2986a00..0536a2ea2 100644 --- a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -25,7 +25,7 @@ ContactAddedCallback gContactAddedCallback=0; ///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback; -inline btScalar calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1) +btScalar btManifoldResult::calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1) { btScalar friction = body0->getRollingFriction() * body1->getRollingFriction(); @@ -58,6 +58,22 @@ btScalar btManifoldResult::calculateCombinedRestitution(const btCollisionObject* return body0->getRestitution() * body1->getRestitution(); } +btScalar btManifoldResult::calculateCombinedContactDamping(const btCollisionObject* body0,const btCollisionObject* body1) +{ + return body0->getContactDamping() + body1->getContactDamping(); +} + +btScalar btManifoldResult::calculateCombinedContactStiffness(const btCollisionObject* body0,const btCollisionObject* body1) +{ + + btScalar s0 = body0->getContactStiffness(); + btScalar s1 = body1->getContactStiffness(); + + btScalar tmp0 = btScalar(1)/s0; + btScalar tmp1 = btScalar(1)/s1; + btScalar combinedStiffness = btScalar(1) / (tmp0+tmp1); + return combinedStiffness; +} btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) @@ -109,6 +125,15 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b newPt.m_combinedFriction = calculateCombinedFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); newPt.m_combinedRollingFriction = calculateCombinedRollingFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); + + if ( (m_body0Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING) || + (m_body1Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING)) + { + newPt.m_combinedContactDamping1 = calculateCombinedContactDamping(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); + newPt.m_combinedContactStiffness1 = calculateCombinedContactStiffness(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); + newPt.m_contactPointFlags |= BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING; + } + btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2); diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/src/BulletCollision/CollisionDispatch/btManifoldResult.h index 977b9a02f..66d11417d 100644 --- a/src/BulletCollision/CollisionDispatch/btManifoldResult.h +++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.h @@ -145,6 +145,9 @@ public: /// in the future we can let the user override the methods to combine restitution and friction static btScalar calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1); static btScalar calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1); + static btScalar calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1); + static btScalar calculateCombinedContactDamping(const btCollisionObject* body0,const btCollisionObject* body1); + static btScalar calculateCombinedContactStiffness(const btCollisionObject* body0,const btCollisionObject* body1); }; #endif //BT_MANIFOLD_RESULT_H diff --git a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index 1bb7a7b99..5ae072ea0 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -40,6 +40,7 @@ enum btContactPointFlags BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED=1, BT_CONTACT_FLAG_HAS_CONTACT_CFM=2, BT_CONTACT_FLAG_HAS_CONTACT_ERP=4, + BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING = 8, }; /// ManifoldContactPoint collects and maintains persistent contactpoints. @@ -116,8 +117,18 @@ class btManifoldPoint btScalar m_appliedImpulseLateral2; btScalar m_contactMotion1; btScalar m_contactMotion2; - btScalar m_contactCFM; - btScalar m_contactERP; + + union + { + btScalar m_contactCFM; + btScalar m_combinedContactStiffness1; + }; + + union + { + btScalar m_contactERP; + btScalar m_combinedContactDamping1; + }; btScalar m_frictionCFM; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 5e1677e27..57ea4e69e 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -777,10 +777,35 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra relaxation = infoGlobal.m_sor; btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep; - btScalar cfm = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)?cp.m_contactCFM:infoGlobal.m_globalCfm; - cfm *= invTimeStep; - btScalar erp = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)?cp.m_contactERP:infoGlobal.m_erp2; + //cfm = 1 / ( dt * kp + kd ) + //erp = dt * kp / ( dt * kp + kd ) + + btScalar cfm = infoGlobal.m_globalCfm; + btScalar erp = infoGlobal.m_erp2; + + if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)) + { + if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) + cfm = cp.m_contactCFM; + if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP) + erp = cp.m_contactERP; + } else + { + if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING) + { + btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 ); + if (denom < SIMD_EPSILON) + { + denom = SIMD_EPSILON; + } + cfm = btScalar(1) / denom; + erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom; + } + } + + cfm *= invTimeStep; + btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); @@ -1053,8 +1078,8 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) { -//disabled: only a single rollingFriction per manifold -// rollingFriction--; + //only a single rollingFriction per manifold + //rollingFriction--; if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold) { relAngVel.normalize(); @@ -1068,6 +1093,9 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); btVector3 axis0,axis1; btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1); + axis0.normalize(); + axis1.normalize(); + applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index 119a24c60..01a59890c 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -299,7 +299,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr //compute rhs and remaining solverConstraint fields - btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop; + btScalar penetration = isFriction? 0 : posError; btScalar rel_vel = 0.f; int ndofA = 0; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index b40c6d72b..9633acaab 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -224,11 +224,34 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol relaxation = infoGlobal.m_sor; btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep; - btScalar cfm = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)?cp.m_contactCFM:infoGlobal.m_globalCfm; - cfm *= invTimeStep; - - btScalar erp = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)?cp.m_contactERP:infoGlobal.m_erp2; + + //cfm = 1 / ( dt * kp + kd ) + //erp = dt * kp / ( dt * kp + kd ) + + btScalar cfm = infoGlobal.m_globalCfm; + btScalar erp = infoGlobal.m_erp2; + if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)) + { + if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) + cfm = cp.m_contactCFM; + if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP) + erp = cp.m_contactERP; + } else + { + if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING) + { + btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 ); + if (denom < SIMD_EPSILON) + { + denom = SIMD_EPSILON; + } + cfm = btScalar(1) / denom; + erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom; + } + } + + cfm *= invTimeStep; @@ -565,12 +588,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult relaxation = infoGlobal.m_sor; btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep; - btScalar cfm = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)?cp.m_contactCFM:infoGlobal.m_globalCfm; - cfm *= invTimeStep; - - btScalar erp = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)?cp.m_contactERP:infoGlobal.m_erp2; - - if (multiBodyA) @@ -713,7 +730,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult - btScalar d = denom0+denom1+cfm; + btScalar d = denom0+denom1+infoGlobal.m_globalCfm; if (d>SIMD_EPSILON) { solverConstraint.m_jacDiagABInv = relaxation/(d); @@ -731,7 +748,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult btScalar restitution = 0.f; - btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop; + btScalar penetration = isFriction? 0 : cp.getDistance(); btScalar rel_vel = 0.f; int ndofA = 0; @@ -790,15 +807,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult if (penetration>0) { - positionalError = 0; velocityError -= penetration / infoGlobal.m_timeStep; - - } else - { - positionalError = -penetration * erp/infoGlobal.m_timeStep; - } + } - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; btScalar velocityImpulse = velocityError*solverConstraint.m_jacDiagABInv; solverConstraint.m_rhs = velocityImpulse; @@ -806,7 +817,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult solverConstraint.m_lowerLimit = -solverConstraint.m_friction; solverConstraint.m_upperLimit = solverConstraint.m_friction; - solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv; + solverConstraint.m_cfm = infoGlobal.m_globalCfm*solverConstraint.m_jacDiagABInv; @@ -951,45 +962,6 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* #define ENABLE_FRICTION #ifdef ENABLE_FRICTION solverConstraint.m_frictionIndex = frictionIndex; -//#define ROLLING_FRICTION -#ifdef ROLLING_FRICTION - int rollingFriction=1; - btVector3 angVelA(0,0,0),angVelB(0,0,0); - if (mbA) - angVelA = mbA->getVelocityVector()>getLink(fcA->m_link).l>getAngularVelocity(); - if (mbB) - angVelB = mbB->getAngularVelocity(); - btVector3 relAngVel = angVelB-angVelA; - - if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) - { -//disabled: only a single rollingFriction per manifold -//rollingFriction--; - if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold) - { - relAngVel.normalize(); - applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - if (relAngVel.length()>0.001) - addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } else - { - addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - btVector3 axis0,axis1; - btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1); - applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - if (axis0.length()>0.001) - addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - if (axis1.length()>0.001) - addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } - } -#endif //ROLLING_FRICTION ///Bullet has several options to set the friction directions ///By default, each contact has only a single friction direction that is recomputed automatically every frame From de5d1b5cfc0d5299f5377e802d0810b56d4bd5dc Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Sat, 3 Sep 2016 08:39:40 -0700 Subject: [PATCH 02/14] make husky wheels soft/compliant reduce shadow size to make VR on older GPU (Mac Pro 2013) perform more smooth less sleep in physics thread. --- data/husky/husky.urdf | 28 +++++++++++++++++++ .../OpenGLWindow/GLInstancingRenderer.cpp | 8 +++--- .../SharedMemory/PhysicsServerExample.cpp | 1 + 3 files changed, 33 insertions(+), 4 deletions(-) diff --git a/data/husky/husky.urdf b/data/husky/husky.urdf index e9762701b..4fab377ae 100644 --- a/data/husky/husky.urdf +++ b/data/husky/husky.urdf @@ -102,6 +102,13 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI + + + + + + + @@ -146,6 +153,13 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI + + + + + + + @@ -190,6 +204,13 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI + + + + + + + @@ -234,6 +255,13 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI + + + + + + + diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index e9c5e38fa..5107ed758 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp @@ -17,9 +17,9 @@ subject to the following restrictions: ///todo: make this configurable in the gui bool useShadowMap = true;// true;//false;//true; -int shadowMapWidth=4096;//8192; -int shadowMapHeight= 4096; -float shadowMapWorldSize=25; +int shadowMapWidth= 2048;//8192; +int shadowMapHeight= 2048; +float shadowMapWorldSize=5; #define MAX_POINTS_IN_BATCH 1024 #define MAX_LINES_IN_BATCH 1024 @@ -1475,7 +1475,7 @@ void GLInstancingRenderer::renderSceneInternal(int renderMode) // m_data->m_shadowMap->disable(); // return; glEnable(GL_CULL_FACE); - glCullFace(GL_BACK); // Cull back-facing triangles -> draw only front-facing triangles + glCullFace(GL_FRONT); // Cull back-facing triangles -> draw only front-facing triangles b3Assert(glGetError() ==GL_NO_ERROR); } else diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 0ff5b4f93..73901c659 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -139,6 +139,7 @@ void MotionThreadFunc(void* userPtr,void* lsMemory) skip++; skip1++; + if (skip1>5) { b3Clock::usleep(250); } From 630fcda38bced545e89a64d70be62ad139b313fe Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 3 Sep 2016 09:53:21 -0700 Subject: [PATCH 03/14] fix compile issue pybullet on MSVC 2010 --- examples/pybullet/pybullet.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 8bc52cfbe..592d3d4e3 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1124,6 +1124,7 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) struct b3ContactInformation contactPointData; b3SharedMemoryStatusHandle statusHandle; int statusType; + int i; PyObject* pyResultList=0; @@ -1148,7 +1149,7 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) b3SetContactFilterBodyA(commandHandle,objectUniqueIdA); b3SetContactFilterBodyB(commandHandle,objectUniqueIdB); b3SubmitClientCommand(sm, commandHandle); - int i; + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType==CMD_CONTACT_POINT_INFORMATION_COMPLETED) From d5ec5ca9a0e4a02b51e7f5e4971d05d89c33fae0 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 6 Sep 2016 13:07:06 -0700 Subject: [PATCH 04/14] Update IDConfig.hpp --- src/BulletInverseDynamics/IDConfig.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/BulletInverseDynamics/IDConfig.hpp b/src/BulletInverseDynamics/IDConfig.hpp index 8e6577912..c34f98941 100644 --- a/src/BulletInverseDynamics/IDConfig.hpp +++ b/src/BulletInverseDynamics/IDConfig.hpp @@ -31,6 +31,10 @@ #include "IDErrorMessages.hpp" #ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H +/* +#include "IDConfigEigen.hpp" +#include "IDConfigBuiltin.hpp" +*/ #define INVDYN_INCLUDE_HELPER_2(x) #x #define INVDYN_INCLUDE_HELPER(x) INVDYN_INCLUDE_HELPER_2(x) #include INVDYN_INCLUDE_HELPER(BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H) From 4944aca28bca28bf2650dfebece6e5e926b88a15 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 6 Sep 2016 13:26:08 -0700 Subject: [PATCH 05/14] Update our_gl.cpp --- examples/TinyRenderer/our_gl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/TinyRenderer/our_gl.cpp b/examples/TinyRenderer/our_gl.cpp index 639db9b0b..eb5a05a78 100644 --- a/examples/TinyRenderer/our_gl.cpp +++ b/examples/TinyRenderer/our_gl.cpp @@ -59,7 +59,7 @@ Vec3f barycentric(Vec2f A, Vec2f B, Vec2f C, Vec2f P) { return Vec3f(-1,1,1); // in this case generate negative coordinates, it will be thrown away by the rasterizator } -void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix, int objectIndex) +void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix) { triangle(clipc,shader,image,zbuffer,0,viewPortMatrix,0); } From edef18e161718a404781677a3630bec26fcb37d2 Mon Sep 17 00:00:00 2001 From: Jeffrey Bingham Date: Tue, 6 Sep 2016 23:27:34 -0700 Subject: [PATCH 06/14] [python] Convert physics calls to double precision. In order to feed Bullet the correct values when compiled with double precision the pybullet interface needs to pass double precision values. --- examples/pybullet/pybullet.c | 302 +++++++++++++++++------------------ 1 file changed, 151 insertions(+), 151 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 8bc52cfbe..3b87ea258 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -14,7 +14,7 @@ #if PY_MAJOR_VERSION >= 3 #define PyInt_FromLong PyLong_FromLong #define PyString_FromString PyBytes_FromString -#endif +#endif enum eCONNECT_METHOD { @@ -46,7 +46,7 @@ pybullet_stepSimulation(PyObject *self, PyObject *args) { statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitStepSimulationCommand(sm)); statusType = b3GetStatusType(statusHandle); - } + } } Py_INCREF(Py_None); @@ -61,7 +61,7 @@ pybullet_connectPhysicsServer(PyObject *self, PyObject *args) PyErr_SetString(SpamError, "Already connected to physics server, disconnect first."); return NULL; } - + { int method=eCONNECT_GUI; if (!PyArg_ParseTuple(args, "i", &method)) @@ -102,10 +102,10 @@ pybullet_connectPhysicsServer(PyObject *self, PyObject *args) return NULL; } }; - - + + } - + Py_INCREF(Py_None); return Py_None; } @@ -122,7 +122,7 @@ pybullet_disconnectPhysicsServer(PyObject *self, PyObject *args) b3DisconnectSharedMemory(sm); sm = 0; } - + Py_INCREF(Py_None); return Py_None; } @@ -135,19 +135,19 @@ pybullet_disconnectPhysicsServer(PyObject *self, PyObject *args) static PyObject * pybullet_loadURDF(PyObject* self, PyObject* args) { - + int size= PySequence_Size(args); - + int bodyIndex = -1; const char* urdfFileName=""; - - float startPosX =0; - float startPosY =0; - float startPosZ = 0; - float startOrnX = 0; - float startOrnY = 0; - float startOrnZ = 0; - float startOrnW = 1; + + double startPosX = 0.0; + double startPosY = 0.0; + double startPosZ = 0.0; + double startOrnX = 0.0; + double startOrnY = 0.0; + double startOrnZ = 0.0; + double startOrnW = 1.0; if (0==sm) { @@ -157,26 +157,26 @@ pybullet_loadURDF(PyObject* self, PyObject* args) if (size==1) { if (!PyArg_ParseTuple(args, "s", &urdfFileName)) - return NULL; + return NULL; } if (size == 4) { - if (!PyArg_ParseTuple(args, "sfff", &urdfFileName, + if (!PyArg_ParseTuple(args, "sddd", &urdfFileName, &startPosX,&startPosY,&startPosZ)) return NULL; } if (size==8) { - if (!PyArg_ParseTuple(args, "sfffffff", &urdfFileName, + if (!PyArg_ParseTuple(args, "sddddddd", &urdfFileName, &startPosX,&startPosY,&startPosZ, &startOrnX,&startOrnY,&startOrnZ, &startOrnW)) return NULL; } - + if (strlen(urdfFileName)) { // printf("(%f, %f, %f) (%f, %f, %f, %f)\n", startPosX,startPosY,startPosZ,startOrnX, startOrnY,startOrnZ, startOrnW); - + b3SharedMemoryStatusHandle statusHandle; int statusType; b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName); @@ -200,11 +200,11 @@ pybullet_loadURDF(PyObject* self, PyObject* args) return PyLong_FromLong(bodyIndex); } -static float pybullet_internalGetFloatFromSequence(PyObject* seq, int index) +static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) { - float v = 0.f; + double v = 0.f; PyObject* item; - + if (PyList_Check(seq)) { item = PyList_GET_ITEM(seq, index); @@ -264,7 +264,7 @@ pybullet_loadSDF(PyObject* self, PyObject* args) pylist = PyTuple_New(numBodies); - if (numBodies >0 && numBodies <= MAX_SDF_BODIES) + if (numBodies >0 && numBodies <= MAX_SDF_BODIES) { for (i=0;i= numJoints) || (jointIndex < 0)) { PyErr_SetString(SpamError, "Joint index out-of-range."); return NULL; } - + commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, targetValue); @@ -809,10 +809,10 @@ pybullet_resetBasePositionAndOrientation(PyObject* self, PyObject* args) PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - + + size= PySequence_Size(args); - + if (size==3) { int bodyIndex; @@ -820,12 +820,12 @@ pybullet_resetBasePositionAndOrientation(PyObject* self, PyObject* args) PyObject* ornObj; double pos[3]; double orn[4];//as a quaternion - + if (PyArg_ParseTuple(args, "iOO", &bodyIndex, &posObj, &ornObj)) { b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; - + { PyObject* seq; int len,i; @@ -865,9 +865,9 @@ pybullet_resetBasePositionAndOrientation(PyObject* self, PyObject* args) } Py_DECREF(seq); } - + commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); - + b3CreatePoseCommandSetBasePosition( commandHandle, pos[0],pos[1],pos[2]); b3CreatePoseCommandSetBaseOrientation( commandHandle, orn[0],orn[1],orn[2],orn[3]); @@ -875,7 +875,7 @@ pybullet_resetBasePositionAndOrientation(PyObject* self, PyObject* args) Py_INCREF(Py_None); return Py_None; } - + } PyErr_SetString(SpamError, "error in resetJointState."); return NULL; @@ -892,21 +892,21 @@ pybullet_resetBasePositionAndOrientation(PyObject* self, PyObject* args) // index, name, type, q-index, u-index, // flags, joint damping, joint friction // -// The format of the returned list is +// The format of the returned list is // [int, str, int, int, int, int, float, float] // // TODO(hellojas): get joint positions for a body static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args) { - PyObject *pyListJointInfo; - + PyObject *pyListJointInfo; + struct b3JointInfo info; - + int bodyIndex = -1; int jointIndex = -1; int jointInfoSize = 8; //size of struct b3JointInfo - + int size= PySequence_Size(args); if (0==sm) @@ -921,7 +921,7 @@ pybullet_getJointInfo(PyObject* self, PyObject* args) { if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) { - + // printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex); pyListJointInfo = PyTuple_New(jointInfoSize); @@ -931,8 +931,8 @@ pybullet_getJointInfo(PyObject* self, PyObject* args) // printf("Joint%d %s, type %d, at q-index %d and u-index %d\n", // info.m_jointIndex, - // info.m_jointName, - // info.m_jointType, + // info.m_jointName, + // info.m_jointType, // info.m_qIndex, // info.m_uIndex); // printf(" flags=%d jointDamping=%f jointFriction=%f\n", @@ -964,7 +964,7 @@ pybullet_getJointInfo(PyObject* self, PyObject* args) } } } - + Py_INCREF(Py_None); return Py_None; } @@ -980,26 +980,26 @@ pybullet_getJointInfo(PyObject* self, PyObject* args) // position, velocity, force torque (6 values), and motor torque // The returned pylist is an array of [float, float, float[6], float] -// TODO(hellojas): check accuracy of position and velocity +// TODO(hellojas): check accuracy of position and velocity // TODO(hellojas): check force torque values static PyObject* pybullet_getJointState(PyObject* self, PyObject* args) { - PyObject *pyListJointForceTorque; - PyObject *pyListJointState; - PyObject *item; - + PyObject *pyListJointForceTorque; + PyObject *pyListJointState; + PyObject *item; + struct b3JointInfo info; struct b3JointSensorState sensorState; - + int bodyIndex = -1; int jointIndex = -1; int sensorStateSize = 4; // size of struct b3JointSensorState int forceTorqueSize = 6; // size of force torque list from b3JointSensorState int i, j; - - + + int size= PySequence_Size(args); if (0==sm) @@ -1017,7 +1017,7 @@ pybullet_getJointState(PyObject* self, PyObject* args) b3RequestActualStateCommandInit(sm, bodyIndex); b3SharedMemoryStatusHandle status_handle = b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); - + status_type = b3GetStatusType(status_handle); if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { @@ -1028,25 +1028,25 @@ pybullet_getJointState(PyObject* self, PyObject* args) pyListJointState = PyTuple_New(sensorStateSize); pyListJointForceTorque = PyTuple_New(forceTorqueSize); - + b3GetJointState(sm, status_handle, jointIndex, &sensorState); - - PyTuple_SetItem(pyListJointState, 0, + + PyTuple_SetItem(pyListJointState, 0, PyFloat_FromDouble(sensorState.m_jointPosition)); - PyTuple_SetItem(pyListJointState, 1, + PyTuple_SetItem(pyListJointState, 1, PyFloat_FromDouble(sensorState.m_jointVelocity)); - + for (j = 0; j < forceTorqueSize; j++) { PyTuple_SetItem(pyListJointForceTorque, j, PyFloat_FromDouble(sensorState.m_jointForceTorque[j])); } - - PyTuple_SetItem(pyListJointState, 2, + + PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque); - - PyTuple_SetItem(pyListJointState, 3, + + PyTuple_SetItem(pyListJointState, 3, PyFloat_FromDouble(sensorState.m_jointMotorTorque)); - + return pyListJointState; } } else @@ -1054,7 +1054,7 @@ pybullet_getJointState(PyObject* self, PyObject* args) PyErr_SetString(SpamError, "getJointState expects 2 arguments (objectUniqueId and joint index)."); return NULL; } - + Py_INCREF(Py_None); return Py_None; } @@ -1092,7 +1092,7 @@ static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) // a view and projection matrix in renderImage() // // // Args: -// matrix - float[16] which will be set by values from objMat +// vector - float[3] which will be set by values from objMat static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) { int i, len; @@ -1256,7 +1256,7 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) int size= PySequence_Size(args); float viewMatrix[16]; float projectionMatrix[16]; - + float cameraPos[3]; float targetPos[3]; float cameraUp[3]; @@ -1267,7 +1267,7 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) // inialize cmd b3SharedMemoryCommandHandle command; - + if (0==sm) { @@ -1353,16 +1353,16 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) { int upAxisIndex=1; float camDistance,yaw,pitch,roll; - + //sometimes more arguments are better :-) if (PyArg_ParseTuple(args, "iiOffffifff", &width, &height, &objTargetPos, &camDistance, &yaw, &pitch, &roll, &upAxisIndex, &nearVal, &farVal, &fov)) { - + b3RequestCameraImageSetPixelResolution(command,width,height); if (pybullet_internalSetVector(objTargetPos, targetPos)) { //printf("width = %d, height = %d, targetPos = %f,%f,%f, distance = %f, yaw = %f, pitch = %f, upAxisIndex = %d, near=%f, far=%f, fov=%f\n",width,height,targetPos[0],targetPos[1],targetPos[2],camDistance,yaw,pitch,upAxisIndex,nearVal,farVal,fov); - + b3RequestCameraImageSetViewMatrix2(command,targetPos,camDistance,yaw,pitch,roll,upAxisIndex); aspect = width/height; b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, nearVal, farVal); @@ -1374,10 +1374,10 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) { PyErr_SetString(SpamError, "Error parsing arguments"); } - - - - + + + + } else { @@ -1389,9 +1389,9 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) { b3SharedMemoryStatusHandle statusHandle; int statusType; - + //b3RequestCameraImageSelectRenderer(command,ER_BULLET_HARDWARE_OPENGL); - + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); if (statusType==CMD_CAMERA_IMAGE_COMPLETED) @@ -1401,7 +1401,7 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) PyObject *pylistRGB; PyObject* pylistDep; PyObject* pylistSeg; - + int i, j, p; b3GetCameraImageData(sm, &imageData); @@ -1433,8 +1433,8 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) item2 = PyLong_FromLong(imageData.m_segmentationMaskValues[depIndex]); PyTuple_SetItem(pylistSeg, depIndex, item2); } - - + + for (p=0; p euler yaw/pitch/roll convention from URDF/SDF, see Gazebo ///https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, PyObject* args) { - + double squ; double sqx; double sqy; double sqz; - + double quat[4]; PyObject* quatObj; - + if (PyArg_ParseTuple(args, "O", &quatObj)) { PyObject* seq; @@ -1727,7 +1727,7 @@ static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, PyObject* args) return Py_None; } -///Given an object id, joint positions, joint velocities and joint accelerations, +///Given an object id, joint positions, joint velocities and joint accelerations, ///compute the joint forces using Inverse Dynamics static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* args) { @@ -1797,7 +1797,7 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg jointForcesOutput); { { - + int i; pylist = PyTuple_New(dofCount); for (i = 0; i Date: Tue, 6 Sep 2016 23:43:57 -0700 Subject: [PATCH 07/14] [format] Convert pybullet.c to consistent style. --- examples/pybullet/pybullet.c | 3042 ++++++++++++++++------------------ 1 file changed, 1438 insertions(+), 1604 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 3b87ea258..161ecf982 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2,129 +2,109 @@ #include "../SharedMemory/PhysicsDirectC_API.h" #include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h" - - #ifdef __APPLE__ #include #else #include #endif - #if PY_MAJOR_VERSION >= 3 #define PyInt_FromLong PyLong_FromLong #define PyString_FromString PyBytes_FromString #endif -enum eCONNECT_METHOD -{ - eCONNECT_GUI=1, - eCONNECT_DIRECT=2, - eCONNECT_SHARED_MEMORY=3, +enum eCONNECT_METHOD { + eCONNECT_GUI = 1, + eCONNECT_DIRECT = 2, + eCONNECT_SHARED_MEMORY = 3, }; - - -static PyObject *SpamError; -static b3PhysicsClientHandle sm=0; +static PyObject* SpamError; +static b3PhysicsClientHandle sm = 0; // Step through one timestep of the simulation -static PyObject * -pybullet_stepSimulation(PyObject *self, PyObject *args) -{ - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; +static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args) { + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + if (b3CanSubmitCommand(sm)) { + statusHandle = b3SubmitClientCommandAndWaitStatus( + sm, b3InitStepSimulationCommand(sm)); + statusType = b3GetStatusType(statusHandle); } + } - { - b3SharedMemoryStatusHandle statusHandle; - int statusType; - - if (b3CanSubmitCommand(sm)) - { - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitStepSimulationCommand(sm)); - statusType = b3GetStatusType(statusHandle); - } - } - - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } -static PyObject * -pybullet_connectPhysicsServer(PyObject *self, PyObject *args) -{ - if (0!=sm) - { - PyErr_SetString(SpamError, "Already connected to physics server, disconnect first."); - return NULL; +static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args) { + if (0 != sm) { + PyErr_SetString(SpamError, + "Already connected to physics server, disconnect first."); + return NULL; + } + + { + int method = eCONNECT_GUI; + if (!PyArg_ParseTuple(args, "i", &method)) { + PyErr_SetString(SpamError, + "connectPhysicsServer expected argument eCONNECT_GUI, " + "eCONNECT_DIRECT or eCONNECT_SHARED_MEMORY"); + return NULL; } - { - int method=eCONNECT_GUI; - if (!PyArg_ParseTuple(args, "i", &method)) - { - PyErr_SetString(SpamError, "connectPhysicsServer expected argument eCONNECT_GUI, eCONNECT_DIRECT or eCONNECT_SHARED_MEMORY"); - return NULL; - } - - switch (method) - { - case eCONNECT_GUI: - { - int argc=0; - char* argv[1]={0}; + switch (method) { + case eCONNECT_GUI: { + int argc = 0; + char* argv[1] = {0}; #ifdef __APPLE__ - sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv); + sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv); #else - sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv); + sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv); #endif - break; - } - case eCONNECT_DIRECT: - { - sm = b3ConnectPhysicsDirect(); - break; - } - case eCONNECT_SHARED_MEMORY: - { - sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY); - break; - } + break; + } + case eCONNECT_DIRECT: { + sm = b3ConnectPhysicsDirect(); + break; + } + case eCONNECT_SHARED_MEMORY: { + sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY); + break; + } + default: { + PyErr_SetString(SpamError, "connectPhysicsServer unexpected argument"); + return NULL; + } + }; + } - default: - { - PyErr_SetString(SpamError, "connectPhysicsServer unexpected argument"); - return NULL; - } - }; - - - } - - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } -static PyObject * -pybullet_disconnectPhysicsServer(PyObject *self, PyObject *args) -{ - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - { - b3DisconnectSharedMemory(sm); - sm = 0; - } +static PyObject* pybullet_disconnectPhysicsServer(PyObject* self, + PyObject* args) { + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + { + b3DisconnectSharedMemory(sm); + sm = 0; + } - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } // Load a URDF file indicating the links and joints of an object @@ -132,755 +112,657 @@ pybullet_disconnectPhysicsServer(PyObject *self, PyObject *args) // to position (0,0,1) with orientation(0,0,0,1) // els(x,y,z) or // loadURDF(pos_x, pos_y, pos_z, orn_x, orn_y, orn_z, orn_w) -static PyObject * -pybullet_loadURDF(PyObject* self, PyObject* args) -{ +static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args) { + int size = PySequence_Size(args); - int size= PySequence_Size(args); + int bodyIndex = -1; + const char* urdfFileName = ""; - int bodyIndex = -1; - const char* urdfFileName=""; + double startPosX = 0.0; + double startPosY = 0.0; + double startPosZ = 0.0; + double startOrnX = 0.0; + double startOrnY = 0.0; + double startOrnZ = 0.0; + double startOrnW = 1.0; - double startPosX = 0.0; - double startPosY = 0.0; - double startPosZ = 0.0; - double startOrnX = 0.0; - double startOrnY = 0.0; - double startOrnZ = 0.0; - double startOrnW = 1.0; + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + if (size == 1) { + if (!PyArg_ParseTuple(args, "s", &urdfFileName)) return NULL; + } + if (size == 4) { + if (!PyArg_ParseTuple(args, "sddd", &urdfFileName, &startPosX, &startPosY, + &startPosZ)) + return NULL; + } + if (size == 8) { + if (!PyArg_ParseTuple(args, "sddddddd", &urdfFileName, &startPosX, + &startPosY, &startPosZ, &startOrnX, &startOrnY, + &startOrnZ, &startOrnW)) + return NULL; + } - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; + if (strlen(urdfFileName)) { + // printf("(%f, %f, %f) (%f, %f, %f, %f)\n", + // startPosX,startPosY,startPosZ,startOrnX, startOrnY,startOrnZ, startOrnW); + + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle command = + b3LoadUrdfCommandInit(sm, urdfFileName); + + // setting the initial position, orientation and other arguments are + // optional + b3LoadUrdfCommandSetStartPosition(command, startPosX, startPosY, startPosZ); + b3LoadUrdfCommandSetStartOrientation(command, startOrnX, startOrnY, + startOrnZ, startOrnW); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType != CMD_URDF_LOADING_COMPLETED) { + PyErr_SetString(SpamError, "Cannot load URDF file."); + return NULL; } - if (size==1) - { - if (!PyArg_ParseTuple(args, "s", &urdfFileName)) - return NULL; - } - if (size == 4) - { - if (!PyArg_ParseTuple(args, "sddd", &urdfFileName, - &startPosX,&startPosY,&startPosZ)) - return NULL; - } - if (size==8) - { - if (!PyArg_ParseTuple(args, "sddddddd", &urdfFileName, - &startPosX,&startPosY,&startPosZ, - &startOrnX,&startOrnY,&startOrnZ, &startOrnW)) - return NULL; - } - - if (strlen(urdfFileName)) - { - // printf("(%f, %f, %f) (%f, %f, %f, %f)\n", startPosX,startPosY,startPosZ,startOrnX, startOrnY,startOrnZ, startOrnW); - - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName); - - //setting the initial position, orientation and other arguments are optional - b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ); - b3LoadUrdfCommandSetStartOrientation(command, startOrnX, startOrnY,startOrnZ, startOrnW ); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - statusType = b3GetStatusType(statusHandle); - if (statusType!=CMD_URDF_LOADING_COMPLETED) - { - PyErr_SetString(SpamError, "Cannot load URDF file."); - return NULL; - } - bodyIndex = b3GetStatusBodyIndex(statusHandle); - } else - { - PyErr_SetString(SpamError, "Empty filename, method expects 1, 4 or 8 arguments."); - return NULL; - } - return PyLong_FromLong(bodyIndex); + bodyIndex = b3GetStatusBodyIndex(statusHandle); + } else { + PyErr_SetString(SpamError, + "Empty filename, method expects 1, 4 or 8 arguments."); + return NULL; + } + return PyLong_FromLong(bodyIndex); } -static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) -{ - double v = 0.f; - PyObject* item; +static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) { + double v = 0.0; + PyObject* item; - if (PyList_Check(seq)) - { - item = PyList_GET_ITEM(seq, index); - v = PyFloat_AsDouble(item); - } - else - { - item = PyTuple_GET_ITEM(seq,index); - v = PyFloat_AsDouble(item); - } - return v; + if (PyList_Check(seq)) { + item = PyList_GET_ITEM(seq, index); + v = PyFloat_AsDouble(item); + } else { + item = PyTuple_GET_ITEM(seq, index); + v = PyFloat_AsDouble(item); + } + return v; } - #define MAX_SDF_BODIES 512 -static PyObject* -pybullet_loadSDF(PyObject* self, PyObject* args) -{ - const char* sdfFileName=""; - int size= PySequence_Size(args); - int numBodies = 0; - int i; - int bodyIndicesOut[MAX_SDF_BODIES]; - PyObject* pylist=0; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle commandHandle; +static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args) { + const char* sdfFileName = ""; + int size = PySequence_Size(args); + int numBodies = 0; + int i; + int bodyIndicesOut[MAX_SDF_BODIES]; + PyObject* pylist = 0; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle commandHandle; - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } - if (size==1) - { - if (!PyArg_ParseTuple(args, "s", &sdfFileName)) - return NULL; - } + if (size == 1) { + if (!PyArg_ParseTuple(args, "s", &sdfFileName)) return NULL; + } - commandHandle = b3LoadSdfCommandInit(sm, sdfFileName); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - if (statusType!=CMD_SDF_LOADING_COMPLETED) - { - PyErr_SetString(SpamError, "Cannot load SDF file."); - return NULL; - } + commandHandle = b3LoadSdfCommandInit(sm, sdfFileName); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType != CMD_SDF_LOADING_COMPLETED) { + PyErr_SetString(SpamError, "Cannot load SDF file."); + return NULL; + } - numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES); - if (numBodies > MAX_SDF_BODIES) - { - PyErr_SetString(SpamError, "SDF exceeds body capacity"); - return NULL; - } + numBodies = + b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES); + if (numBodies > MAX_SDF_BODIES) { + PyErr_SetString(SpamError, "SDF exceeds body capacity"); + return NULL; + } - pylist = PyTuple_New(numBodies); + pylist = PyTuple_New(numBodies); - if (numBodies >0 && numBodies <= MAX_SDF_BODIES) - { - for (i=0;i 0 && numBodies <= MAX_SDF_BODIES) { + for (i = 0; i < numBodies; i++) { + PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i])); + } + } + return pylist; } // Reset the simulation to remove all loaded objects -static PyObject * -pybullet_resetSimulation(PyObject* self, PyObject* args) -{ - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - { - b3SharedMemoryStatusHandle statusHandle; - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm)); - } - Py_INCREF(Py_None); - return Py_None; -} - -static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) -{ - int size; - int bodyIndex, jointIndex, controlMode; - - double targetPosition=0; - double targetVelocity=0; - double maxForce=100000; - double appliedForce = 0; - double kp=0.1; - double kd=1.0; - int valid = 0; - - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - size= PySequence_Size(args); - if (size==4) - { - double targetValue = 0; - // see switch statement below for convertsions dependent on controlMode - if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue)) - { - PyErr_SetString(SpamError, "Error parsing arguments"); - return NULL; - } - valid = 1; - switch (controlMode) - { - case CONTROL_MODE_POSITION_VELOCITY_PD: - { - targetPosition = targetValue; - break; - } - case CONTROL_MODE_VELOCITY: - { - targetVelocity = targetValue; - break; - } - case CONTROL_MODE_TORQUE: - { - appliedForce = targetValue; - break; - } - default: - { - valid = 0; - } - } - - } - if (size==5) - { - double targetValue = 0; - //See switch statement for conversions - if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce)) - { - PyErr_SetString(SpamError, "Error parsing arguments"); - return NULL; - } - valid = 1; - - switch (controlMode) - { - case CONTROL_MODE_POSITION_VELOCITY_PD: - { - targetPosition = targetValue; - break; - } - case CONTROL_MODE_VELOCITY: - { - targetVelocity = targetValue; - break; - } - case CONTROL_MODE_TORQUE: - { - valid = 0; - break; - } - default: - { - valid = 0; - } - } - } - if (size==6) - { - double gain; - double targetValue = 0; - if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &gain)) - { - PyErr_SetString(SpamError, "Error parsing arguments"); - return NULL; - } - valid = 1; - - switch (controlMode) - { - case CONTROL_MODE_POSITION_VELOCITY_PD: - { - targetPosition = targetValue; - kp = gain; - break; - } - case CONTROL_MODE_VELOCITY: - { - targetVelocity = targetValue; - kd = gain; - break; - } - case CONTROL_MODE_TORQUE: - { - valid = 0; - break; - } - default: - { - valid = 0; - } - } - } - if (size==8) - { - // only applicable for CONTROL_MODE_POSITION_VELOCITY_PD. - if (!PyArg_ParseTuple(args, "iiiddddd", &bodyIndex, &jointIndex, &controlMode, &targetPosition, &targetVelocity, &maxForce, &kp, &kd)) - { - PyErr_SetString(SpamError, "Error parsing arguments"); - return NULL; - } - valid = 1; - } - - - - if (valid) - { - - int numJoints; - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - struct b3JointInfo info; - - numJoints = b3GetNumJoints(sm,bodyIndex); - if ((jointIndex >= numJoints) || (jointIndex < 0)) - { - PyErr_SetString(SpamError, "Joint index out-of-range."); - return NULL; - } - - if ((controlMode != CONTROL_MODE_VELOCITY) && - (controlMode != CONTROL_MODE_TORQUE) && - (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) - { - PyErr_SetString(SpamError, "Illegral control mode."); - return NULL; - } - - commandHandle = b3JointControlCommandInit2(sm, bodyIndex,controlMode); - - b3GetJointInfo(sm, bodyIndex, jointIndex, &info); - - switch (controlMode) - { - case CONTROL_MODE_VELOCITY: - { - b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity); - b3JointControlSetKd(commandHandle, info.m_uIndex, kd); - b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); - break; - } - - case CONTROL_MODE_TORQUE: - { - b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, appliedForce); - break; - } - - case CONTROL_MODE_POSITION_VELOCITY_PD: - { - b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition); - b3JointControlSetKp(commandHandle, info.m_uIndex, kp); - b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity); - b3JointControlSetKd(commandHandle, info.m_uIndex, kd); - b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); - break; - } - default: - { - } - }; - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - - Py_INCREF(Py_None); - return Py_None; - } - PyErr_SetString(SpamError, "Error parsing arguments in setJointControl."); +static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args) { + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; + } + + { + b3SharedMemoryStatusHandle statusHandle; + statusHandle = b3SubmitClientCommandAndWaitStatus( + sm, b3InitResetSimulationCommand(sm)); + } + Py_INCREF(Py_None); + return Py_None; } -static PyObject * -pybullet_setRealTimeSimulation(PyObject* self, PyObject* args) -{ - if (0 == sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } +static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) { + int size; + int bodyIndex, jointIndex, controlMode; - { - int enableRealTimeSimulation = 0; - int ret; + double targetPosition = 0.0; + double targetVelocity = 0.0; + double maxForce = 100000.0; + double appliedForce = 0.0; + double kp = 0.1; + double kd = 1.0; + int valid = 0; - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); - b3SharedMemoryStatusHandle statusHandle; + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } - if (!PyArg_ParseTuple(args, "i", &enableRealTimeSimulation)) - { - PyErr_SetString(SpamError, "setRealTimeSimulation expected a single value (integer)."); - return NULL; - } - ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation); + size = PySequence_Size(args); + if (size == 4) { + double targetValue = 0.0; + // see switch statement below for convertsions dependent on controlMode + if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, + &targetValue)) { + PyErr_SetString(SpamError, "Error parsing arguments"); + return NULL; + } + valid = 1; + switch (controlMode) { + case CONTROL_MODE_POSITION_VELOCITY_PD: { + targetPosition = targetValue; + break; + } + case CONTROL_MODE_VELOCITY: { + targetVelocity = targetValue; + break; + } + case CONTROL_MODE_TORQUE: { + appliedForce = targetValue; + break; + } + default: { valid = 0; } + } + } + if (size == 5) { + double targetValue = 0.0; + // See switch statement for conversions + if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, + &targetValue, &maxForce)) { + PyErr_SetString(SpamError, "Error parsing arguments"); + return NULL; + } + valid = 1; - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - //ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); - } + switch (controlMode) { + case CONTROL_MODE_POSITION_VELOCITY_PD: { + targetPosition = targetValue; + break; + } + case CONTROL_MODE_VELOCITY: { + targetVelocity = targetValue; + break; + } + case CONTROL_MODE_TORQUE: { + valid = 0; + break; + } + default: { valid = 0; } + } + } + if (size == 6) { + double gain = 0.0; + double targetValue = 0.0; + if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, + &targetValue, &maxForce, &gain)) { + PyErr_SetString(SpamError, "Error parsing arguments"); + return NULL; + } + valid = 1; - Py_INCREF(Py_None); - return Py_None; -} + switch (controlMode) { + case CONTROL_MODE_POSITION_VELOCITY_PD: { + targetPosition = targetValue; + kp = gain; + break; + } + case CONTROL_MODE_VELOCITY: { + targetVelocity = targetValue; + kd = gain; + break; + } + case CONTROL_MODE_TORQUE: { + valid = 0; + break; + } + default: { valid = 0; } + } + } + if (size == 8) { + // only applicable for CONTROL_MODE_POSITION_VELOCITY_PD. + if (!PyArg_ParseTuple(args, "iiiddddd", &bodyIndex, &jointIndex, + &controlMode, &targetPosition, &targetVelocity, + &maxForce, &kp, &kd)) { + PyErr_SetString(SpamError, "Error parsing arguments"); + return NULL; + } + valid = 1; + } + if (valid) { + int numJoints; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + struct b3JointInfo info; - - -// Set the gravity of the world with (x, y, z) arguments -static PyObject * -pybullet_setGravity(PyObject* self, PyObject* args) -{ - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; + numJoints = b3GetNumJoints(sm, bodyIndex); + if ((jointIndex >= numJoints) || (jointIndex < 0)) { + PyErr_SetString(SpamError, "Joint index out-of-range."); + return NULL; } - { - double gravX=0; - double gravY=0; - double gravZ=-10; - int ret; - - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); - b3SharedMemoryStatusHandle statusHandle; - - if (!PyArg_ParseTuple(args, "ddd", &gravX,&gravY,&gravZ)) - { - PyErr_SetString(SpamError, "setGravity expected (x,y,z) values."); - return NULL; - } - ret = b3PhysicsParamSetGravity(command, gravX,gravY, gravZ); - //ret = b3PhysicsParamSetTimeStep(command, timeStep); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - //ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); - } - - Py_INCREF(Py_None); - return Py_None; -} - -static PyObject * -pybullet_setTimeStep(PyObject* self, PyObject* args) -{ - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; + if ((controlMode != CONTROL_MODE_VELOCITY) && + (controlMode != CONTROL_MODE_TORQUE) && + (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) { + PyErr_SetString(SpamError, "Illegral control mode."); + return NULL; } - { - double timeStep=0.001; - int ret; + commandHandle = b3JointControlCommandInit2(sm, bodyIndex, controlMode); - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); - b3SharedMemoryStatusHandle statusHandle; + b3GetJointInfo(sm, bodyIndex, jointIndex, &info); - if (!PyArg_ParseTuple(args, "d", &timeStep)) - { - PyErr_SetString(SpamError, "setTimeStep expected a single value (double)."); - return NULL; - } - ret = b3PhysicsParamSetTimeStep(command, timeStep); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - //ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); - } + switch (controlMode) { + case CONTROL_MODE_VELOCITY: { + b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, + targetVelocity); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); + break; + } + + case CONTROL_MODE_TORQUE: { + b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, + appliedForce); + break; + } + + case CONTROL_MODE_POSITION_VELOCITY_PD: { + b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, + targetPosition); + b3JointControlSetKp(commandHandle, info.m_uIndex, kp); + b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, + targetVelocity); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); + break; + } + default: {} + }; + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); Py_INCREF(Py_None); return Py_None; + } + PyErr_SetString(SpamError, "Error parsing arguments in setJointControl."); + return NULL; } +static PyObject* pybullet_setRealTimeSimulation(PyObject* self, + PyObject* args) { + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + { + int enableRealTimeSimulation = 0; + int ret; + + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; + + if (!PyArg_ParseTuple(args, "i", &enableRealTimeSimulation)) { + PyErr_SetString( + SpamError, + "setRealTimeSimulation expected a single value (integer)."); + return NULL; + } + ret = + b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); + } + + Py_INCREF(Py_None); + return Py_None; +} + +// Set the gravity of the world with (x, y, z) arguments +static PyObject* pybullet_setGravity(PyObject* self, PyObject* args) { + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + double gravX = 0.0; + double gravY = 0.0; + double gravZ = -10.0; + int ret; + + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; + + if (!PyArg_ParseTuple(args, "ddd", &gravX, &gravY, &gravZ)) { + PyErr_SetString(SpamError, "setGravity expected (x,y,z) values."); + return NULL; + } + ret = b3PhysicsParamSetGravity(command, gravX, gravY, gravZ); + // ret = b3PhysicsParamSetTimeStep(command, timeStep); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); + } + + Py_INCREF(Py_None); + return Py_None; +} + +static PyObject* pybullet_setTimeStep(PyObject* self, PyObject* args) { + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + double timeStep = 0.001; + int ret; + + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; + + if (!PyArg_ParseTuple(args, "d", &timeStep)) { + PyErr_SetString(SpamError, + "setTimeStep expected a single value (double)."); + return NULL; + } + ret = b3PhysicsParamSetTimeStep(command, timeStep); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); + } + + Py_INCREF(Py_None); + return Py_None; +} // Internal function used to get the base position and orientation // Orientation is returned in quaternions -static int pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double basePosition[3],double baseOrientation[3]) -{ - basePosition[0] = 0.; - basePosition[1] = 0.; - basePosition[2] = 0.; +static int pybullet_internalGetBasePositionAndOrientation( + int bodyIndex, double basePosition[3], double baseOrientation[3]) { + basePosition[0] = 0.; + basePosition[1] = 0.; + basePosition[2] = 0.; - baseOrientation[0] = 0.; - baseOrientation[1] = 0.; - baseOrientation[2] = 0.; - baseOrientation[3] = 1.; + baseOrientation[0] = 0.; + baseOrientation[1] = 0.; + baseOrientation[2] = 0.; + baseOrientation[3] = 1.; - { + { + { + b3SharedMemoryCommandHandle cmd_handle = + b3RequestActualStateCommandInit(sm, bodyIndex); + b3SharedMemoryStatusHandle status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); - { - b3SharedMemoryCommandHandle cmd_handle = - b3RequestActualStateCommandInit(sm, bodyIndex); - b3SharedMemoryStatusHandle status_handle = - b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + const int status_type = b3GetStatusType(status_handle); + const double* actualStateQ; + // const double* jointReactionForces[]; + int i; - const int status_type = b3GetStatusType(status_handle); - const double* actualStateQ; - // const double* jointReactionForces[]; - int i; + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { + PyErr_SetString(SpamError, "getBasePositionAndOrientation failed."); + return 0; + } - if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) - { - PyErr_SetString(SpamError, "getBasePositionAndOrientation failed."); - return 0; - } - - b3GetStatusActualState(status_handle, 0/* body_unique_id */, - 0/* num_degree_of_freedom_q */, - 0/* num_degree_of_freedom_u */, 0 /*root_local_inertial_frame*/, - &actualStateQ , 0 /* actual_state_q_dot */, - 0 /* joint_reaction_forces */); + b3GetStatusActualState( + status_handle, 0 /* body_unique_id */, + 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */, + 0 /*root_local_inertial_frame*/, &actualStateQ, + 0 /* actual_state_q_dot */, 0 /* joint_reaction_forces */); - // printf("joint reaction forces="); - // for (i=0; i < (sizeof(jointReactionForces)/sizeof(double)); i++) { - // printf("%f ", jointReactionForces[i]); - // } - //now, position x,y,z = actualStateQ[0],actualStateQ[1],actualStateQ[2] - //and orientation x,y,z,w = actualStateQ[3],actualStateQ[4],actualStateQ[5],actualStateQ[6] - basePosition[0] = actualStateQ[0]; - basePosition[1] = actualStateQ[1]; - basePosition[2] = actualStateQ[2]; + // printf("joint reaction forces="); + // for (i=0; i < (sizeof(jointReactionForces)/sizeof(double)); i++) { + // printf("%f ", jointReactionForces[i]); + // } + // now, position x,y,z = actualStateQ[0],actualStateQ[1],actualStateQ[2] + // and orientation x,y,z,w = + // actualStateQ[3],actualStateQ[4],actualStateQ[5],actualStateQ[6] + basePosition[0] = actualStateQ[0]; + basePosition[1] = actualStateQ[1]; + basePosition[2] = actualStateQ[2]; - baseOrientation[0] = actualStateQ[3]; - baseOrientation[1] = actualStateQ[4]; - baseOrientation[2] = actualStateQ[5]; - baseOrientation[3] = actualStateQ[6]; - - } - } - return 1; + baseOrientation[0] = actualStateQ[3]; + baseOrientation[1] = actualStateQ[4]; + baseOrientation[2] = actualStateQ[5]; + baseOrientation[3] = actualStateQ[6]; + } + } + return 1; } // Get the positions (x,y,z) and orientation (x,y,z,w) in quaternion // values for the base link of your object // Object is retrieved based on body index, which is the order // the object was loaded into the simulation (0-based) -static PyObject * -pybullet_getBasePositionAndOrientation(PyObject* self, PyObject* args) -{ - int bodyIndex = -1; - double basePosition[3]; - double baseOrientation[4]; - PyObject *pylistPos; - PyObject *pylistOrientation; +static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self, + PyObject* args) { + int bodyIndex = -1; + double basePosition[3]; + double baseOrientation[4]; + PyObject* pylistPos; + PyObject* pylistOrientation; - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (!PyArg_ParseTuple(args, "i", &bodyIndex)) { + PyErr_SetString(SpamError, "Expected a body index (integer)."); + return NULL; + } + + if (0 == pybullet_internalGetBasePositionAndOrientation( + bodyIndex, basePosition, baseOrientation)) { + PyErr_SetString(SpamError, + "GetBasePositionAndOrientation failed (#joints/links " + "exceeds maximum?)."); + return NULL; + } + + { + PyObject* item; + int i; + int num = 3; + pylistPos = PyTuple_New(num); + for (i = 0; i < num; i++) { + item = PyFloat_FromDouble(basePosition[i]); + PyTuple_SetItem(pylistPos, i, item); } + } - if (!PyArg_ParseTuple(args, "i", &bodyIndex )) - { - PyErr_SetString(SpamError, "Expected a body index (integer)."); - return NULL; - } - - if (0==pybullet_internalGetBasePositionAndOrientation(bodyIndex, basePosition, baseOrientation)) - { - PyErr_SetString(SpamError, "GetBasePositionAndOrientation failed (#joints/links exceeds maximum?)."); - return NULL; - } - - { - - PyObject *item; - int i; - int num=3; - pylistPos = PyTuple_New(num); - for (i = 0; i < num; i++) - { - item = PyFloat_FromDouble(basePosition[i]); - PyTuple_SetItem(pylistPos, i, item); - } - - } - - { - - PyObject *item; - int i; - int num=4; - pylistOrientation = PyTuple_New(num); - for (i = 0; i < num; i++) - { - item = PyFloat_FromDouble(baseOrientation[i]); - PyTuple_SetItem(pylistOrientation, i, item); - } - - } - - { - PyObject *pylist; - pylist = PyTuple_New(2); - PyTuple_SetItem(pylist,0,pylistPos); - PyTuple_SetItem(pylist,1,pylistOrientation); - return pylist; + { + PyObject* item; + int i; + int num = 4; + pylistOrientation = PyTuple_New(num); + for (i = 0; i < num; i++) { + item = PyFloat_FromDouble(baseOrientation[i]); + PyTuple_SetItem(pylistOrientation, i, item); } + } + { + PyObject* pylist; + pylist = PyTuple_New(2); + PyTuple_SetItem(pylist, 0, pylistPos); + PyTuple_SetItem(pylist, 1, pylistOrientation); + return pylist; + } } // Return the number of joints in an object based on // body index; body index is based on order of sequence // the object is loaded into simulation -static PyObject * -pybullet_getNumJoints(PyObject* self, PyObject* args) -{ - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } +static PyObject* pybullet_getNumJoints(PyObject* self, PyObject* args) { + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } - { - int bodyIndex = -1; - int numJoints=0; - if (!PyArg_ParseTuple(args, "i", &bodyIndex )) - { - PyErr_SetString(SpamError, "Expected a body index (integer)."); - return NULL; - } - numJoints = b3GetNumJoints(sm,bodyIndex); + { + int bodyIndex = -1; + int numJoints = 0; + if (!PyArg_ParseTuple(args, "i", &bodyIndex)) { + PyErr_SetString(SpamError, "Expected a body index (integer)."); + return NULL; + } + numJoints = b3GetNumJoints(sm, bodyIndex); #if PY_MAJOR_VERSION >= 3 - return PyLong_FromLong(numJoints); + return PyLong_FromLong(numJoints); #else - return PyInt_FromLong(numJoints); + return PyInt_FromLong(numJoints); #endif - } + } } // Initalize all joint positions given a list of values -static PyObject* -pybullet_resetJointState(PyObject* self, PyObject* args) -{ - int size; - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - - size= PySequence_Size(args); - - if (size==3) - { - int bodyIndex; - int jointIndex; - double targetValue; - - if (PyArg_ParseTuple(args, "iid", &bodyIndex, &jointIndex, &targetValue)) - { - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int numJoints; - - numJoints = b3GetNumJoints(sm,bodyIndex); - if ((jointIndex >= numJoints) || (jointIndex < 0)) - { - PyErr_SetString(SpamError, "Joint index out-of-range."); - return NULL; - } - - commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); - - b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, targetValue); - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - Py_INCREF(Py_None); - return Py_None; - } - - } - PyErr_SetString(SpamError, "error in resetJointState."); +static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args) { + int size; + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; -} + } -// Reset the position and orientation of the base/root link, position [x,y,z] and orientation quaternion [x,y,z,w] -static PyObject* -pybullet_resetBasePositionAndOrientation(PyObject* self, PyObject* args) -{ - int size; - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); + size = PySequence_Size(args); + + if (size == 3) { + int bodyIndex; + int jointIndex; + double targetValue; + + if (PyArg_ParseTuple(args, "iid", &bodyIndex, &jointIndex, &targetValue)) { + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int numJoints; + + numJoints = b3GetNumJoints(sm, bodyIndex); + if ((jointIndex >= numJoints) || (jointIndex < 0)) { + PyErr_SetString(SpamError, "Joint index out-of-range."); return NULL; + } + + commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); + + b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, + targetValue); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + Py_INCREF(Py_None); + return Py_None; } - - - size= PySequence_Size(args); - - if (size==3) - { - int bodyIndex; - PyObject* posObj; - PyObject* ornObj; - double pos[3]; - double orn[4];//as a quaternion - - if (PyArg_ParseTuple(args, "iOO", &bodyIndex, &posObj, &ornObj)) - { - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - - { - PyObject* seq; - int len,i; - seq = PySequence_Fast(posObj, "expected a sequence"); - len = PySequence_Size(posObj); - if (len==3) - { - for (i = 0; i < 3; i++) - { - pos[i] = pybullet_internalGetFloatFromSequence(seq,i); - } - } else - { - PyErr_SetString(SpamError, "position needs a 3 coordinates [x,y,z]."); - Py_DECREF(seq); - return NULL; - } - Py_DECREF(seq); - } - - { - PyObject* seq; - int len,i; - seq = PySequence_Fast(ornObj, "expected a sequence"); - len = PySequence_Size(ornObj); - if (len==4) - { - for (i = 0; i < 4; i++) - { - orn[i] = pybullet_internalGetFloatFromSequence(seq,i); - } - } else - { - PyErr_SetString(SpamError, "orientation needs a 4 coordinates, quaternion [x,y,z,w]."); - Py_DECREF(seq); - return NULL; - } - Py_DECREF(seq); - } - - commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); - - b3CreatePoseCommandSetBasePosition( commandHandle, pos[0],pos[1],pos[2]); - b3CreatePoseCommandSetBaseOrientation( commandHandle, orn[0],orn[1],orn[2],orn[3]); - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - Py_INCREF(Py_None); - return Py_None; - } - - } - PyErr_SetString(SpamError, "error in resetJointState."); - return NULL; + } + PyErr_SetString(SpamError, "error in resetJointState."); + return NULL; } +// Reset the position and orientation of the base/root link, position [x,y,z] +// and orientation quaternion [x,y,z,w] +static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self, + PyObject* args) { + int size; + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + size = PySequence_Size(args); + + if (size == 3) { + int bodyIndex; + PyObject* posObj; + PyObject* ornObj; + double pos[3]; + double orn[4]; // as a quaternion + + if (PyArg_ParseTuple(args, "iOO", &bodyIndex, &posObj, &ornObj)) { + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + + { + PyObject* seq; + int len, i; + seq = PySequence_Fast(posObj, "expected a sequence"); + len = PySequence_Size(posObj); + if (len == 3) { + for (i = 0; i < 3; i++) { + pos[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + } else { + PyErr_SetString(SpamError, "position needs a 3 coordinates [x,y,z]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } + + { + PyObject* seq; + int len, i; + seq = PySequence_Fast(ornObj, "expected a sequence"); + len = PySequence_Size(ornObj); + if (len == 4) { + for (i = 0; i < 4; i++) { + orn[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + } else { + PyErr_SetString( + SpamError, + "orientation needs a 4 coordinates, quaternion [x,y,z,w]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } + + commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); + + b3CreatePoseCommandSetBasePosition(commandHandle, pos[0], pos[1], pos[2]); + b3CreatePoseCommandSetBaseOrientation(commandHandle, orn[0], orn[1], + orn[2], orn[3]); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + Py_INCREF(Py_None); + return Py_None; + } + } + PyErr_SetString(SpamError, "error in resetJointState."); + return NULL; +} // Get the a single joint info for a specific bodyIndex // @@ -896,80 +778,63 @@ pybullet_resetBasePositionAndOrientation(PyObject* self, PyObject* args) // [int, str, int, int, int, int, float, float] // // TODO(hellojas): get joint positions for a body -static PyObject* -pybullet_getJointInfo(PyObject* self, PyObject* args) -{ - PyObject *pyListJointInfo; +static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args) { + PyObject* pyListJointInfo; struct b3JointInfo info; int bodyIndex = -1; int jointIndex = -1; - int jointInfoSize = 8; //size of struct b3JointInfo + int jointInfoSize = 8; // size of struct b3JointInfo - int size= PySequence_Size(args); + int size = PySequence_Size(args); - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } - - - if (size==2) // get body index and joint index + if (size == 2) // get body index and joint index { - if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) - { - + if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) { // printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex); - pyListJointInfo = PyTuple_New(jointInfoSize); + pyListJointInfo = PyTuple_New(jointInfoSize); - if (b3GetJointInfo(sm, bodyIndex, jointIndex, &info)) - { - - // printf("Joint%d %s, type %d, at q-index %d and u-index %d\n", - // info.m_jointIndex, - // info.m_jointName, - // info.m_jointType, - // info.m_qIndex, - // info.m_uIndex); - // printf(" flags=%d jointDamping=%f jointFriction=%f\n", - // info.m_flags, - // info.m_jointDamping, - // info.m_jointFriction); - PyTuple_SetItem(pyListJointInfo, 0, - PyInt_FromLong(info.m_jointIndex)); - PyTuple_SetItem(pyListJointInfo, 1, - PyString_FromString(info.m_jointName)); - PyTuple_SetItem(pyListJointInfo, 2, - PyInt_FromLong(info.m_jointType)); - PyTuple_SetItem(pyListJointInfo, 3, - PyInt_FromLong(info.m_qIndex)); - PyTuple_SetItem(pyListJointInfo, 4, - PyInt_FromLong(info.m_uIndex)); - PyTuple_SetItem(pyListJointInfo, 5, - PyInt_FromLong(info.m_flags)); - PyTuple_SetItem(pyListJointInfo, 6, - PyFloat_FromDouble(info.m_jointDamping)); - PyTuple_SetItem(pyListJointInfo, 7, - PyFloat_FromDouble(info.m_jointFriction)); - return pyListJointInfo; - } - else - { - PyErr_SetString(SpamError, "GetJointInfo failed."); - return NULL; - } + if (b3GetJointInfo(sm, bodyIndex, jointIndex, &info)) { + // printf("Joint%d %s, type %d, at q-index %d and u-index %d\n", + // info.m_jointIndex, + // info.m_jointName, + // info.m_jointType, + // info.m_qIndex, + // info.m_uIndex); + // printf(" flags=%d jointDamping=%f jointFriction=%f\n", + // info.m_flags, + // info.m_jointDamping, + // info.m_jointFriction); + PyTuple_SetItem(pyListJointInfo, 0, PyInt_FromLong(info.m_jointIndex)); + PyTuple_SetItem(pyListJointInfo, 1, + PyString_FromString(info.m_jointName)); + PyTuple_SetItem(pyListJointInfo, 2, PyInt_FromLong(info.m_jointType)); + PyTuple_SetItem(pyListJointInfo, 3, PyInt_FromLong(info.m_qIndex)); + PyTuple_SetItem(pyListJointInfo, 4, PyInt_FromLong(info.m_uIndex)); + PyTuple_SetItem(pyListJointInfo, 5, PyInt_FromLong(info.m_flags)); + PyTuple_SetItem(pyListJointInfo, 6, + PyFloat_FromDouble(info.m_jointDamping)); + PyTuple_SetItem(pyListJointInfo, 7, + PyFloat_FromDouble(info.m_jointFriction)); + return pyListJointInfo; + } else { + PyErr_SetString(SpamError, "GetJointInfo failed."); + return NULL; + } } } - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } - // Returns the state of a specific joint in a given bodyIndex // // Args: @@ -983,108 +848,96 @@ pybullet_getJointInfo(PyObject* self, PyObject* args) // TODO(hellojas): check accuracy of position and velocity // TODO(hellojas): check force torque values -static PyObject* -pybullet_getJointState(PyObject* self, PyObject* args) -{ - PyObject *pyListJointForceTorque; - PyObject *pyListJointState; - PyObject *item; +static PyObject* pybullet_getJointState(PyObject* self, PyObject* args) { + PyObject* pyListJointForceTorque; + PyObject* pyListJointState; + PyObject* item; struct b3JointInfo info; struct b3JointSensorState sensorState; int bodyIndex = -1; int jointIndex = -1; - int sensorStateSize = 4; // size of struct b3JointSensorState - int forceTorqueSize = 6; // size of force torque list from b3JointSensorState + int sensorStateSize = 4; // size of struct b3JointSensorState + int forceTorqueSize = 6; // size of force torque list from b3JointSensorState int i, j; + int size = PySequence_Size(args); - int size= PySequence_Size(args); - - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - if (size==2) // get body index and joint index - { - if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) - { - int status_type = 0; - b3SharedMemoryCommandHandle cmd_handle = - b3RequestActualStateCommandInit(sm, bodyIndex); - b3SharedMemoryStatusHandle status_handle = - b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); - - status_type = b3GetStatusType(status_handle); - if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) - { - PyErr_SetString(SpamError, "getBasePositionAndOrientation failed."); - return NULL; - } - - pyListJointState = PyTuple_New(sensorStateSize); - pyListJointForceTorque = PyTuple_New(forceTorqueSize); - - - b3GetJointState(sm, status_handle, jointIndex, &sensorState); - - PyTuple_SetItem(pyListJointState, 0, - PyFloat_FromDouble(sensorState.m_jointPosition)); - PyTuple_SetItem(pyListJointState, 1, - PyFloat_FromDouble(sensorState.m_jointVelocity)); - - for (j = 0; j < forceTorqueSize; j++) { - PyTuple_SetItem(pyListJointForceTorque, j, - PyFloat_FromDouble(sensorState.m_jointForceTorque[j])); - } - - PyTuple_SetItem(pyListJointState, 2, - pyListJointForceTorque); - - PyTuple_SetItem(pyListJointState, 3, - PyFloat_FromDouble(sensorState.m_jointMotorTorque)); - - return pyListJointState; - } - } else - { - PyErr_SetString(SpamError, "getJointState expects 2 arguments (objectUniqueId and joint index)."); - return NULL; + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; } - Py_INCREF(Py_None); - return Py_None; + if (size == 2) // get body index and joint index + { + if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) { + int status_type = 0; + b3SharedMemoryCommandHandle cmd_handle = + b3RequestActualStateCommandInit(sm, bodyIndex); + b3SharedMemoryStatusHandle status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + + status_type = b3GetStatusType(status_handle); + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { + PyErr_SetString(SpamError, "getBasePositionAndOrientation failed."); + return NULL; + } + + pyListJointState = PyTuple_New(sensorStateSize); + pyListJointForceTorque = PyTuple_New(forceTorqueSize); + + b3GetJointState(sm, status_handle, jointIndex, &sensorState); + + PyTuple_SetItem(pyListJointState, 0, + PyFloat_FromDouble(sensorState.m_jointPosition)); + PyTuple_SetItem(pyListJointState, 1, + PyFloat_FromDouble(sensorState.m_jointVelocity)); + + for (j = 0; j < forceTorqueSize; j++) { + PyTuple_SetItem(pyListJointForceTorque, j, + PyFloat_FromDouble(sensorState.m_jointForceTorque[j])); + } + + PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque); + + PyTuple_SetItem(pyListJointState, 3, + PyFloat_FromDouble(sensorState.m_jointMotorTorque)); + + return pyListJointState; + } + } else { + PyErr_SetString( + SpamError, + "getJointState expects 2 arguments (objectUniqueId and joint index)."); + return NULL; + } + + Py_INCREF(Py_None); + return Py_None; } - - // internal function to set a float matrix[16] // used to initialize camera position with // a view and projection matrix in renderImage() // // // Args: // matrix - float[16] which will be set by values from objMat -static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) -{ - int i, len; - PyObject* seq; +static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) { + int i, len; + PyObject* seq; - seq = PySequence_Fast(objMat, "expected a sequence"); - len = PySequence_Size(objMat); - if (len==16) - { - for (i = 0; i < len; i++) - { - matrix[i] = pybullet_internalGetFloatFromSequence(seq,i); - } - Py_DECREF(seq); - return 1; + seq = PySequence_Fast(objMat, "expected a sequence"); + len = PySequence_Size(objMat); + if (len == 16) { + for (i = 0; i < len; i++) { + matrix[i] = pybullet_internalGetFloatFromSequence(seq, i); } Py_DECREF(seq); - return 0; + return 1; + } + Py_DECREF(seq); + return 0; } // internal function to set a float vector[3] @@ -1093,136 +946,139 @@ static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) // // // Args: // vector - float[3] which will be set by values from objMat -static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) -{ - int i, len; - PyObject* seq; +static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) { + int i, len; + PyObject* seq; - seq = PySequence_Fast(objMat, "expected a sequence"); - len = PySequence_Size(objMat); - if (len==3) - { - for (i = 0; i < len; i++) - { - vector[i] = pybullet_internalGetFloatFromSequence(seq,i); - } - Py_DECREF(seq); - return 1; + seq = PySequence_Fast(objMat, "expected a sequence"); + len = PySequence_Size(objMat); + if (len == 3) { + for (i = 0; i < len; i++) { + vector[i] = pybullet_internalGetFloatFromSequence(seq, i); } Py_DECREF(seq); - return 0; + return 1; + } + Py_DECREF(seq); + return 0; } +static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) { + int size = PySequence_Size(args); + int objectUniqueIdA = -1; + int objectUniqueIdB = -1; + b3SharedMemoryCommandHandle commandHandle; + struct b3ContactInformation contactPointData; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + PyObject* pyResultList = 0; -static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) -{ - int size= PySequence_Size(args); - int objectUniqueIdA = -1; - int objectUniqueIdB = -1; - b3SharedMemoryCommandHandle commandHandle; - struct b3ContactInformation contactPointData; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - - PyObject* pyResultList=0; - - if (size==1) - { - if (!PyArg_ParseTuple(args, "i", &objectUniqueIdA)) - { - PyErr_SetString(SpamError, "Error parsing object unique id"); - return NULL; - } + if (size == 1) { + if (!PyArg_ParseTuple(args, "i", &objectUniqueIdA)) { + PyErr_SetString(SpamError, "Error parsing object unique id"); + return NULL; } - if (size==2) - { - if (!PyArg_ParseTuple(args, "ii", &objectUniqueIdA,&objectUniqueIdB)) - { - PyErr_SetString(SpamError, "Error parsing object unique id"); - return NULL; - } + } + if (size == 2) { + if (!PyArg_ParseTuple(args, "ii", &objectUniqueIdA, &objectUniqueIdB)) { + PyErr_SetString(SpamError, "Error parsing object unique id"); + return NULL; } - - commandHandle = b3InitRequestContactPointInformation(sm); - b3SetContactFilterBodyA(commandHandle,objectUniqueIdA); - b3SetContactFilterBodyB(commandHandle,objectUniqueIdB); - b3SubmitClientCommand(sm, commandHandle); - int i; - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - if (statusType==CMD_CONTACT_POINT_INFORMATION_COMPLETED) - { - - /* - 0 int m_contactFlags; - 1 int m_bodyUniqueIdA; - 2 int m_bodyUniqueIdB; - 3 int m_linkIndexA; - 4 int m_linkIndexB; - 5-6-7 double m_positionOnAInWS[3];//contact point location on object A, in world space coordinates - 8-9-10 double m_positionOnBInWS[3];//contact point location on object A, in world space coordinates - 11-12-13 double m_contactNormalOnBInWS[3];//the separating contact normal, pointing from object B towards object A - 14 double m_contactDistance;//negative number is penetration, positive is distance. - - 15 double m_normalForce; - */ - - b3GetContactPointInformation(sm, &contactPointData); - pyResultList = PyTuple_New(contactPointData.m_numContactPoints); - for (i=0;i euler yaw/pitch/roll convention from URDF/SDF, see Gazebo +/// https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc +static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, + PyObject* args) { + double squ; + double sqx; + double sqy; + double sqz; -static PyObject* pybullet_getQuaternionFromEuler(PyObject* self, PyObject* args) -{ + double quat[4]; + + PyObject* quatObj; + + if (PyArg_ParseTuple(args, "O", &quatObj)) { + PyObject* seq; + int len, i; + seq = PySequence_Fast(quatObj, "expected a sequence"); + len = PySequence_Size(quatObj); + if (len == 4) { + for (i = 0; i < 4; i++) { + quat[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + } else { + PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } else { + PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w]."); + return NULL; + } + + { double rpy[3]; - - PyObject* eulerObj; - - if (PyArg_ParseTuple(args, "O", &eulerObj)) + double sarg; + sqx = quat[0] * quat[0]; + sqy = quat[1] * quat[1]; + sqz = quat[2] * quat[2]; + squ = quat[3] * quat[3]; + rpy[0] = atan2(2 * (quat[1] * quat[2] + quat[3] * quat[0]), + squ - sqx - sqy + sqz); + sarg = -2 * (quat[0] * quat[2] - quat[3] * quat[1]); + rpy[1] = sarg <= -1.0 ? -0.5 * 3.141592538 + : (sarg >= 1.0 ? 0.5 * 3.141592538 : asin(sarg)); + rpy[2] = atan2(2 * (quat[0] * quat[1] + quat[3] * quat[2]), + squ + sqx - sqy - sqz); { - PyObject* seq; - int len,i; - seq = PySequence_Fast(eulerObj, "expected a sequence"); - len = PySequence_Size(eulerObj); - if (len==3) - { - for (i = 0; i < 3; i++) - { - rpy[i] = pybullet_internalGetFloatFromSequence(seq,i); - } - } else - { - PyErr_SetString(SpamError, "Euler angles need a 3 coordinates [roll, pitch, yaw]."); - Py_DECREF(seq); - return NULL; + PyObject* pylist; + int i; + pylist = PyTuple_New(3); + for (i = 0; i < 3; i++) + PyTuple_SetItem(pylist, i, PyFloat_FromDouble(rpy[i])); + return pylist; + } + } + Py_INCREF(Py_None); + return Py_None; +} + +/// Given an object id, joint positions, joint velocities and joint +/// accelerations, +/// compute the joint forces using Inverse Dynamics +static PyObject* pybullet_calculateInverseDynamics(PyObject* self, + PyObject* args) { + int size; + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + size = PySequence_Size(args); + if (size == 4) { + int bodyIndex; + PyObject* objPositionsQ; + PyObject* objVelocitiesQdot; + PyObject* objAccelerations; + + if (PyArg_ParseTuple(args, "iOOO", &bodyIndex, &objPositionsQ, + &objVelocitiesQdot, &objAccelerations)) { + int szObPos = PySequence_Size(objPositionsQ); + int szObVel = PySequence_Size(objVelocitiesQdot); + int szObAcc = PySequence_Size(objAccelerations); + int numJoints = b3GetNumJoints(sm, bodyIndex); + if (numJoints && (szObPos == numJoints) && (szObVel == numJoints) && + (szObAcc == numJoints)) { + int szInBytes = sizeof(double) * numJoints; + int i; + PyObject* pylist = 0; + double* jointPositionsQ = (double*)malloc(szInBytes); + double* jointVelocitiesQdot = (double*)malloc(szInBytes); + double* jointAccelerations = (double*)malloc(szInBytes); + double* jointForcesOutput = (double*)malloc(szInBytes); + + for (i = 0; i < numJoints; i++) { + jointPositionsQ[i] = + pybullet_internalGetFloatFromSequence(objPositionsQ, i); + jointVelocitiesQdot[i] = + pybullet_internalGetFloatFromSequence(objVelocitiesQdot, i); + jointAccelerations[i] = + pybullet_internalGetFloatFromSequence(objAccelerations, i); } - Py_DECREF(seq); - } else - { - PyErr_SetString(SpamError, "Euler angles need a 3 coordinates [roll, pitch, yaw]."); + + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle commandHandle = + b3CalculateInverseDynamicsCommandInit( + sm, bodyIndex, jointPositionsQ, jointVelocitiesQdot, + jointAccelerations); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) { + int bodyUniqueId; + int dofCount; + + b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId, + &dofCount, 0); + + if (dofCount) { + b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0, + jointForcesOutput); + { + { + int i; + pylist = PyTuple_New(dofCount); + for (i = 0; i < dofCount; i++) + PyTuple_SetItem(pylist, i, + PyFloat_FromDouble(jointForcesOutput[i])); + } + } + } + + } else { + PyErr_SetString(SpamError, + "Internal error in calculateInverseDynamics"); + } + } + free(jointPositionsQ); + free(jointVelocitiesQdot); + free(jointAccelerations); + free(jointForcesOutput); + if (pylist) return pylist; + } else { + PyErr_SetString(SpamError, + "calculateInverseDynamics numJoints needs to be " + "positive and [joint positions], [joint velocities], " + "[joint accelerations] need to match the number of " + "joints."); return NULL; + } + + } else { + PyErr_SetString(SpamError, + "calculateInverseDynamics expects 4 arguments, body " + "index, [joint positions], [joint velocities], [joint " + "accelerations]."); + return NULL; } - - { - double phi, the, psi; - double roll = rpy[0]; - double pitch = rpy[1]; - double yaw = rpy[2]; - phi = roll / 2.0; - the = pitch / 2.0; - psi = yaw / 2.0; - { - double quat[4] = { - sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi), - cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi), - cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi), - cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi)}; - - //normalize the quaternion - double len = sqrt(quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3]); - quat[0] /= len; - quat[1] /= len; - quat[2] /= len; - quat[3] /= len; - { - PyObject *pylist; - int i; - pylist = PyTuple_New(4); - for (i=0;i<4;i++) - PyTuple_SetItem(pylist,i,PyFloat_FromDouble(quat[i])); - return pylist; - } - } - } - Py_INCREF(Py_None); - return Py_None; - + } else { + PyErr_SetString(SpamError, + "calculateInverseDynamics expects 4 arguments, body index, " + "[joint positions], [joint velocities], [joint " + "accelerations]."); + return NULL; + } + Py_INCREF(Py_None); + return Py_None; } -///quaternion <-> euler yaw/pitch/roll convention from URDF/SDF, see Gazebo -///https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc -static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, PyObject* args) -{ - - double squ; - double sqx; - double sqy; - double sqz; - - double quat[4]; - - PyObject* quatObj; - - if (PyArg_ParseTuple(args, "O", &quatObj)) - { - PyObject* seq; - int len,i; - seq = PySequence_Fast(quatObj, "expected a sequence"); - len = PySequence_Size(quatObj); - if (len==4) - { - for (i = 0; i < 4; i++) - { - quat[i] = pybullet_internalGetFloatFromSequence(seq,i); - } - } else - { - PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w]."); - Py_DECREF(seq); - return NULL; - } - Py_DECREF(seq); - } else - { - PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w]."); - return NULL; - } - - { - double rpy[3]; - double sarg; - sqx = quat[0] * quat[0]; - sqy = quat[1] * quat[1]; - sqz = quat[2] * quat[2]; - squ = quat[3] * quat[3]; - rpy[0] = atan2(2 * (quat[1]*quat[2] + quat[3]*quat[0]), squ - sqx - sqy + sqz); - sarg = -2 * (quat[0]*quat[2] - quat[3] * quat[1]); - rpy[1] = sarg <= -1.0 ? -0.5*3.141592538 : (sarg >= 1.0 ? 0.5*3.141592538 : asin(sarg)); - rpy[2] = atan2(2 * (quat[0]*quat[1] + quat[3]*quat[2]), squ + sqx - sqy - sqz); - { - PyObject *pylist; - int i; - pylist = PyTuple_New(3); - for (i=0;i<3;i++) - PyTuple_SetItem(pylist,i,PyFloat_FromDouble(rpy[i])); - return pylist; - } - } - Py_INCREF(Py_None); - return Py_None; -} - -///Given an object id, joint positions, joint velocities and joint accelerations, -///compute the joint forces using Inverse Dynamics -static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* args) -{ - int size; - if (0 == sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - size = PySequence_Size(args); - if (size==4) - { - - int bodyIndex; - PyObject* objPositionsQ; - PyObject* objVelocitiesQdot; - PyObject* objAccelerations; - - if (PyArg_ParseTuple(args, "iOOO", &bodyIndex, &objPositionsQ, &objVelocitiesQdot, &objAccelerations)) - { - int szObPos = PySequence_Size(objPositionsQ); - int szObVel = PySequence_Size(objVelocitiesQdot); - int szObAcc = PySequence_Size(objAccelerations); - int numJoints = b3GetNumJoints(sm, bodyIndex); - if (numJoints && (szObPos == numJoints) && (szObVel == numJoints) && (szObAcc == numJoints)) - { - int szInBytes = sizeof(double)*numJoints; - int i; - PyObject* pylist = 0; - double* jointPositionsQ = (double*)malloc(szInBytes); - double* jointVelocitiesQdot = (double*)malloc(szInBytes); - double* jointAccelerations = (double*)malloc(szInBytes); - double* jointForcesOutput = (double*)malloc(szInBytes); - - for (i = 0; i < numJoints; i++) - { - jointPositionsQ[i] = pybullet_internalGetFloatFromSequence(objPositionsQ, i); - jointVelocitiesQdot[i] = pybullet_internalGetFloatFromSequence(objVelocitiesQdot, i); - jointAccelerations[i] = pybullet_internalGetFloatFromSequence(objAccelerations, i); - } - - { - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(sm, - bodyIndex, jointPositionsQ, jointVelocitiesQdot, jointAccelerations); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - - statusType = b3GetStatusType(statusHandle); - - if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) - { - int bodyUniqueId; - int dofCount; - - b3GetStatusInverseDynamicsJointForces(statusHandle, - &bodyUniqueId, - &dofCount, - 0); - - if (dofCount) - { - b3GetStatusInverseDynamicsJointForces(statusHandle, - 0, - 0, - jointForcesOutput); - { - { - - int i; - pylist = PyTuple_New(dofCount); - for (i = 0; i= 3 - static struct PyModuleDef moduledef = { - PyModuleDef_HEAD_INIT, - "pybullet", /* m_name */ - "Python bindings for Bullet Physics Robotics API (also known as Shared Memory API)", /* m_doc */ - -1, /* m_size */ - SpamMethods, /* m_methods */ - NULL, /* m_reload */ - NULL, /* m_traverse */ - NULL, /* m_clear */ - NULL, /* m_free */ - }; +static struct PyModuleDef moduledef = { + PyModuleDef_HEAD_INIT, "pybullet", /* m_name */ + "Python bindings for Bullet Physics Robotics API (also known as Shared " + "Memory API)", /* m_doc */ + -1, /* m_size */ + SpamMethods, /* m_methods */ + NULL, /* m_reload */ + NULL, /* m_traverse */ + NULL, /* m_clear */ + NULL, /* m_free */ +}; #endif PyMODINIT_FUNC @@ -1953,39 +1789,37 @@ initpybullet(void) #endif { - PyObject *m; + PyObject* m; #if PY_MAJOR_VERSION >= 3 - m = PyModule_Create(&moduledef); + m = PyModule_Create(&moduledef); #else - m = Py_InitModule3("pybullet", - SpamMethods, "Python bindings for Bullet"); + m = Py_InitModule3("pybullet", SpamMethods, "Python bindings for Bullet"); #endif #if PY_MAJOR_VERSION >= 3 - if (m == NULL) - return m; + if (m == NULL) return m; #else - if (m == NULL) - return; + if (m == NULL) return; #endif + PyModule_AddIntConstant(m, "SHARED_MEMORY", + eCONNECT_SHARED_MEMORY); // user read + PyModule_AddIntConstant(m, "DIRECT", eCONNECT_DIRECT); // user read + PyModule_AddIntConstant(m, "GUI", eCONNECT_GUI); // user read - PyModule_AddIntConstant (m, "SHARED_MEMORY", eCONNECT_SHARED_MEMORY); // user read - PyModule_AddIntConstant (m, "DIRECT", eCONNECT_DIRECT); // user read - PyModule_AddIntConstant (m, "GUI", eCONNECT_GUI); // user read + PyModule_AddIntConstant(m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE); + PyModule_AddIntConstant(m, "VELOCITY_CONTROL", + CONTROL_MODE_VELOCITY); // user read + PyModule_AddIntConstant(m, "POSITION_CONTROL", + CONTROL_MODE_POSITION_VELOCITY_PD); // user read - PyModule_AddIntConstant (m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE); - PyModule_AddIntConstant (m, "VELOCITY_CONTROL", CONTROL_MODE_VELOCITY); // user read - PyModule_AddIntConstant (m, "POSITION_CONTROL", CONTROL_MODE_POSITION_VELOCITY_PD); // user read + PyModule_AddIntConstant(m, "LINK_FRAME", EF_LINK_FRAME); + PyModule_AddIntConstant(m, "WORLD_FRAME", EF_WORLD_FRAME); - PyModule_AddIntConstant (m, "LINK_FRAME", EF_LINK_FRAME); - PyModule_AddIntConstant (m, "WORLD_FRAME", EF_WORLD_FRAME); - - SpamError = PyErr_NewException("pybullet.error", NULL, NULL); - Py_INCREF(SpamError); - PyModule_AddObject(m, "error", SpamError); + SpamError = PyErr_NewException("pybullet.error", NULL, NULL); + Py_INCREF(SpamError); + PyModule_AddObject(m, "error", SpamError); #if PY_MAJOR_VERSION >= 3 - return m; + return m; #endif } - From ba2f522e05cec899d42c0493a21a7835f82f713f Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 7 Sep 2016 16:02:16 -0700 Subject: [PATCH 08/14] Update BulletUrdfImporter.cpp use free (and not delete), since b3ImportMeshUtility/stbi_load uses malloc (and not new) --- examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 992840850..efbbbbb89 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -1001,7 +1001,7 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP //delete textures for (int i=0;i Date: Thu, 8 Sep 2016 11:12:58 -0700 Subject: [PATCH 09/14] merge pybullet.c after conflicts due to Jeff's formatting pull request. --- examples/pybullet/pybullet.c | 3139 ++++++++++++++++------------------ 1 file changed, 1486 insertions(+), 1653 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 592d3d4e3..801cf5322 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2,129 +2,109 @@ #include "../SharedMemory/PhysicsDirectC_API.h" #include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h" - - #ifdef __APPLE__ #include #else #include #endif - #if PY_MAJOR_VERSION >= 3 #define PyInt_FromLong PyLong_FromLong #define PyString_FromString PyBytes_FromString -#endif +#endif -enum eCONNECT_METHOD -{ - eCONNECT_GUI=1, - eCONNECT_DIRECT=2, - eCONNECT_SHARED_MEMORY=3, +enum eCONNECT_METHOD { + eCONNECT_GUI = 1, + eCONNECT_DIRECT = 2, + eCONNECT_SHARED_MEMORY = 3, }; - - -static PyObject *SpamError; -static b3PhysicsClientHandle sm=0; +static PyObject* SpamError; +static b3PhysicsClientHandle sm = 0; // Step through one timestep of the simulation -static PyObject * -pybullet_stepSimulation(PyObject *self, PyObject *args) -{ - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; +static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args) { + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + if (b3CanSubmitCommand(sm)) { + statusHandle = b3SubmitClientCommandAndWaitStatus( + sm, b3InitStepSimulationCommand(sm)); + statusType = b3GetStatusType(statusHandle); } + } - { - b3SharedMemoryStatusHandle statusHandle; - int statusType; - - if (b3CanSubmitCommand(sm)) - { - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitStepSimulationCommand(sm)); - statusType = b3GetStatusType(statusHandle); - } - } - - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } -static PyObject * -pybullet_connectPhysicsServer(PyObject *self, PyObject *args) -{ - if (0!=sm) - { - PyErr_SetString(SpamError, "Already connected to physics server, disconnect first."); - return NULL; - } - - { - int method=eCONNECT_GUI; - if (!PyArg_ParseTuple(args, "i", &method)) - { - PyErr_SetString(SpamError, "connectPhysicsServer expected argument eCONNECT_GUI, eCONNECT_DIRECT or eCONNECT_SHARED_MEMORY"); - return NULL; - } +static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args) { + if (0 != sm) { + PyErr_SetString(SpamError, + "Already connected to physics server, disconnect first."); + return NULL; + } - switch (method) - { - case eCONNECT_GUI: - { - int argc=0; - char* argv[1]={0}; + { + int method = eCONNECT_GUI; + if (!PyArg_ParseTuple(args, "i", &method)) { + PyErr_SetString(SpamError, + "connectPhysicsServer expected argument eCONNECT_GUI, " + "eCONNECT_DIRECT or eCONNECT_SHARED_MEMORY"); + return NULL; + } + + switch (method) { + case eCONNECT_GUI: { + int argc = 0; + char* argv[1] = {0}; #ifdef __APPLE__ - sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv); + sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv); #else - sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv); + sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv); #endif - break; - } - case eCONNECT_DIRECT: - { - sm = b3ConnectPhysicsDirect(); - break; - } - case eCONNECT_SHARED_MEMORY: - { - sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY); - break; - } + break; + } + case eCONNECT_DIRECT: { + sm = b3ConnectPhysicsDirect(); + break; + } + case eCONNECT_SHARED_MEMORY: { + sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY); + break; + } + default: { + PyErr_SetString(SpamError, "connectPhysicsServer unexpected argument"); + return NULL; + } + }; + } - default: - { - PyErr_SetString(SpamError, "connectPhysicsServer unexpected argument"); - return NULL; - } - }; - - - } - - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } -static PyObject * -pybullet_disconnectPhysicsServer(PyObject *self, PyObject *args) -{ - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - { - b3DisconnectSharedMemory(sm); - sm = 0; - } - - Py_INCREF(Py_None); - return Py_None; +static PyObject* pybullet_disconnectPhysicsServer(PyObject* self, + PyObject* args) { + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + { + b3DisconnectSharedMemory(sm); + sm = 0; + } + + Py_INCREF(Py_None); + return Py_None; } // Load a URDF file indicating the links and joints of an object @@ -132,755 +112,657 @@ pybullet_disconnectPhysicsServer(PyObject *self, PyObject *args) // to position (0,0,1) with orientation(0,0,0,1) // els(x,y,z) or // loadURDF(pos_x, pos_y, pos_z, orn_x, orn_y, orn_z, orn_w) -static PyObject * -pybullet_loadURDF(PyObject* self, PyObject* args) -{ - - int size= PySequence_Size(args); - - int bodyIndex = -1; - const char* urdfFileName=""; - - float startPosX =0; - float startPosY =0; - float startPosZ = 0; - float startOrnX = 0; - float startOrnY = 0; - float startOrnZ = 0; - float startOrnW = 1; +static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args) { + int size = PySequence_Size(args); - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; + int bodyIndex = -1; + const char* urdfFileName = ""; + + double startPosX = 0.0; + double startPosY = 0.0; + double startPosZ = 0.0; + double startOrnX = 0.0; + double startOrnY = 0.0; + double startOrnZ = 0.0; + double startOrnW = 1.0; + + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + if (size == 1) { + if (!PyArg_ParseTuple(args, "s", &urdfFileName)) return NULL; + } + if (size == 4) { + if (!PyArg_ParseTuple(args, "sddd", &urdfFileName, &startPosX, &startPosY, + &startPosZ)) + return NULL; + } + if (size == 8) { + if (!PyArg_ParseTuple(args, "sddddddd", &urdfFileName, &startPosX, + &startPosY, &startPosZ, &startOrnX, &startOrnY, + &startOrnZ, &startOrnW)) + return NULL; + } + + if (strlen(urdfFileName)) { + // printf("(%f, %f, %f) (%f, %f, %f, %f)\n", + // startPosX,startPosY,startPosZ,startOrnX, startOrnY,startOrnZ, startOrnW); + + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle command = + b3LoadUrdfCommandInit(sm, urdfFileName); + + // setting the initial position, orientation and other arguments are + // optional + b3LoadUrdfCommandSetStartPosition(command, startPosX, startPosY, startPosZ); + b3LoadUrdfCommandSetStartOrientation(command, startOrnX, startOrnY, + startOrnZ, startOrnW); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType != CMD_URDF_LOADING_COMPLETED) { + PyErr_SetString(SpamError, "Cannot load URDF file."); + return NULL; } - if (size==1) - { - if (!PyArg_ParseTuple(args, "s", &urdfFileName)) - return NULL; - } - if (size == 4) - { - if (!PyArg_ParseTuple(args, "sfff", &urdfFileName, - &startPosX,&startPosY,&startPosZ)) - return NULL; - } - if (size==8) - { - if (!PyArg_ParseTuple(args, "sfffffff", &urdfFileName, - &startPosX,&startPosY,&startPosZ, - &startOrnX,&startOrnY,&startOrnZ, &startOrnW)) - return NULL; - } - - if (strlen(urdfFileName)) - { - // printf("(%f, %f, %f) (%f, %f, %f, %f)\n", startPosX,startPosY,startPosZ,startOrnX, startOrnY,startOrnZ, startOrnW); - - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName); - - //setting the initial position, orientation and other arguments are optional - b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ); - b3LoadUrdfCommandSetStartOrientation(command, startOrnX, startOrnY,startOrnZ, startOrnW ); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - statusType = b3GetStatusType(statusHandle); - if (statusType!=CMD_URDF_LOADING_COMPLETED) - { - PyErr_SetString(SpamError, "Cannot load URDF file."); - return NULL; - } - bodyIndex = b3GetStatusBodyIndex(statusHandle); - } else - { - PyErr_SetString(SpamError, "Empty filename, method expects 1, 4 or 8 arguments."); - return NULL; - } - return PyLong_FromLong(bodyIndex); + bodyIndex = b3GetStatusBodyIndex(statusHandle); + } else { + PyErr_SetString(SpamError, + "Empty filename, method expects 1, 4 or 8 arguments."); + return NULL; + } + return PyLong_FromLong(bodyIndex); } -static float pybullet_internalGetFloatFromSequence(PyObject* seq, int index) -{ - float v = 0.f; - PyObject* item; - - if (PyList_Check(seq)) - { - item = PyList_GET_ITEM(seq, index); - v = PyFloat_AsDouble(item); - } - else - { - item = PyTuple_GET_ITEM(seq,index); - v = PyFloat_AsDouble(item); - } - return v; -} +static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) { + double v = 0.0; + PyObject* item; + if (PyList_Check(seq)) { + item = PyList_GET_ITEM(seq, index); + v = PyFloat_AsDouble(item); + } else { + item = PyTuple_GET_ITEM(seq, index); + v = PyFloat_AsDouble(item); + } + return v; +} #define MAX_SDF_BODIES 512 -static PyObject* -pybullet_loadSDF(PyObject* self, PyObject* args) -{ - const char* sdfFileName=""; - int size= PySequence_Size(args); - int numBodies = 0; - int i; - int bodyIndicesOut[MAX_SDF_BODIES]; - PyObject* pylist=0; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle commandHandle; +static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args) { + const char* sdfFileName = ""; + int size = PySequence_Size(args); + int numBodies = 0; + int i; + int bodyIndicesOut[MAX_SDF_BODIES]; + PyObject* pylist = 0; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle commandHandle; - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } - if (size==1) - { - if (!PyArg_ParseTuple(args, "s", &sdfFileName)) - return NULL; - } + if (size == 1) { + if (!PyArg_ParseTuple(args, "s", &sdfFileName)) return NULL; + } - commandHandle = b3LoadSdfCommandInit(sm, sdfFileName); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - if (statusType!=CMD_SDF_LOADING_COMPLETED) - { - PyErr_SetString(SpamError, "Cannot load SDF file."); - return NULL; - } + commandHandle = b3LoadSdfCommandInit(sm, sdfFileName); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType != CMD_SDF_LOADING_COMPLETED) { + PyErr_SetString(SpamError, "Cannot load SDF file."); + return NULL; + } - numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES); - if (numBodies > MAX_SDF_BODIES) - { - PyErr_SetString(SpamError, "SDF exceeds body capacity"); - return NULL; - } + numBodies = + b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES); + if (numBodies > MAX_SDF_BODIES) { + PyErr_SetString(SpamError, "SDF exceeds body capacity"); + return NULL; + } - pylist = PyTuple_New(numBodies); + pylist = PyTuple_New(numBodies); - if (numBodies >0 && numBodies <= MAX_SDF_BODIES) - { - for (i=0;i 0 && numBodies <= MAX_SDF_BODIES) { + for (i = 0; i < numBodies; i++) { + PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i])); + } + } + return pylist; } // Reset the simulation to remove all loaded objects -static PyObject * -pybullet_resetSimulation(PyObject* self, PyObject* args) -{ - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - { - b3SharedMemoryStatusHandle statusHandle; - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm)); - } - Py_INCREF(Py_None); - return Py_None; -} - -static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) -{ - int size; - int bodyIndex, jointIndex, controlMode; - - double targetPosition=0; - double targetVelocity=0; - double maxForce=100000; - double appliedForce = 0; - double kp=0.1; - double kd=1.0; - int valid = 0; - - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - size= PySequence_Size(args); - if (size==4) - { - double targetValue = 0; - // see switch statement below for convertsions dependent on controlMode - if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue)) - { - PyErr_SetString(SpamError, "Error parsing arguments"); - return NULL; - } - valid = 1; - switch (controlMode) - { - case CONTROL_MODE_POSITION_VELOCITY_PD: - { - targetPosition = targetValue; - break; - } - case CONTROL_MODE_VELOCITY: - { - targetVelocity = targetValue; - break; - } - case CONTROL_MODE_TORQUE: - { - appliedForce = targetValue; - break; - } - default: - { - valid = 0; - } - } - - } - if (size==5) - { - double targetValue = 0; - //See switch statement for conversions - if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce)) - { - PyErr_SetString(SpamError, "Error parsing arguments"); - return NULL; - } - valid = 1; - - switch (controlMode) - { - case CONTROL_MODE_POSITION_VELOCITY_PD: - { - targetPosition = targetValue; - break; - } - case CONTROL_MODE_VELOCITY: - { - targetVelocity = targetValue; - break; - } - case CONTROL_MODE_TORQUE: - { - valid = 0; - break; - } - default: - { - valid = 0; - } - } - } - if (size==6) - { - double gain; - double targetValue = 0; - if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &gain)) - { - PyErr_SetString(SpamError, "Error parsing arguments"); - return NULL; - } - valid = 1; - - switch (controlMode) - { - case CONTROL_MODE_POSITION_VELOCITY_PD: - { - targetPosition = targetValue; - kp = gain; - break; - } - case CONTROL_MODE_VELOCITY: - { - targetVelocity = targetValue; - kd = gain; - break; - } - case CONTROL_MODE_TORQUE: - { - valid = 0; - break; - } - default: - { - valid = 0; - } - } - } - if (size==8) - { - // only applicable for CONTROL_MODE_POSITION_VELOCITY_PD. - if (!PyArg_ParseTuple(args, "iiiddddd", &bodyIndex, &jointIndex, &controlMode, &targetPosition, &targetVelocity, &maxForce, &kp, &kd)) - { - PyErr_SetString(SpamError, "Error parsing arguments"); - return NULL; - } - valid = 1; - } - - - - if (valid) - { - - int numJoints; - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - struct b3JointInfo info; - - numJoints = b3GetNumJoints(sm,bodyIndex); - if ((jointIndex >= numJoints) || (jointIndex < 0)) - { - PyErr_SetString(SpamError, "Joint index out-of-range."); - return NULL; - } - - if ((controlMode != CONTROL_MODE_VELOCITY) && - (controlMode != CONTROL_MODE_TORQUE) && - (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) - { - PyErr_SetString(SpamError, "Illegral control mode."); - return NULL; - } - - commandHandle = b3JointControlCommandInit2(sm, bodyIndex,controlMode); - - b3GetJointInfo(sm, bodyIndex, jointIndex, &info); - - switch (controlMode) - { - case CONTROL_MODE_VELOCITY: - { - b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity); - b3JointControlSetKd(commandHandle, info.m_uIndex, kd); - b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); - break; - } - - case CONTROL_MODE_TORQUE: - { - b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, appliedForce); - break; - } - - case CONTROL_MODE_POSITION_VELOCITY_PD: - { - b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition); - b3JointControlSetKp(commandHandle, info.m_uIndex, kp); - b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity); - b3JointControlSetKd(commandHandle, info.m_uIndex, kd); - b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); - break; - } - default: - { - } - }; - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - - Py_INCREF(Py_None); - return Py_None; - } - PyErr_SetString(SpamError, "Error parsing arguments in setJointControl."); +static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args) { + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; + } + + { + b3SharedMemoryStatusHandle statusHandle; + statusHandle = b3SubmitClientCommandAndWaitStatus( + sm, b3InitResetSimulationCommand(sm)); + } + Py_INCREF(Py_None); + return Py_None; } -static PyObject * -pybullet_setRealTimeSimulation(PyObject* self, PyObject* args) -{ - if (0 == sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } +static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) { + int size; + int bodyIndex, jointIndex, controlMode; - { - int enableRealTimeSimulation = 0; - int ret; + double targetPosition = 0.0; + double targetVelocity = 0.0; + double maxForce = 100000.0; + double appliedForce = 0.0; + double kp = 0.1; + double kd = 1.0; + int valid = 0; - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); - b3SharedMemoryStatusHandle statusHandle; + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } - if (!PyArg_ParseTuple(args, "i", &enableRealTimeSimulation)) - { - PyErr_SetString(SpamError, "setRealTimeSimulation expected a single value (integer)."); - return NULL; - } - ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation); + size = PySequence_Size(args); + if (size == 4) { + double targetValue = 0.0; + // see switch statement below for convertsions dependent on controlMode + if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, + &targetValue)) { + PyErr_SetString(SpamError, "Error parsing arguments"); + return NULL; + } + valid = 1; + switch (controlMode) { + case CONTROL_MODE_POSITION_VELOCITY_PD: { + targetPosition = targetValue; + break; + } + case CONTROL_MODE_VELOCITY: { + targetVelocity = targetValue; + break; + } + case CONTROL_MODE_TORQUE: { + appliedForce = targetValue; + break; + } + default: { valid = 0; } + } + } + if (size == 5) { + double targetValue = 0.0; + // See switch statement for conversions + if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, + &targetValue, &maxForce)) { + PyErr_SetString(SpamError, "Error parsing arguments"); + return NULL; + } + valid = 1; - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - //ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); - } + switch (controlMode) { + case CONTROL_MODE_POSITION_VELOCITY_PD: { + targetPosition = targetValue; + break; + } + case CONTROL_MODE_VELOCITY: { + targetVelocity = targetValue; + break; + } + case CONTROL_MODE_TORQUE: { + valid = 0; + break; + } + default: { valid = 0; } + } + } + if (size == 6) { + double gain = 0.0; + double targetValue = 0.0; + if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, + &targetValue, &maxForce, &gain)) { + PyErr_SetString(SpamError, "Error parsing arguments"); + return NULL; + } + valid = 1; - Py_INCREF(Py_None); - return Py_None; -} + switch (controlMode) { + case CONTROL_MODE_POSITION_VELOCITY_PD: { + targetPosition = targetValue; + kp = gain; + break; + } + case CONTROL_MODE_VELOCITY: { + targetVelocity = targetValue; + kd = gain; + break; + } + case CONTROL_MODE_TORQUE: { + valid = 0; + break; + } + default: { valid = 0; } + } + } + if (size == 8) { + // only applicable for CONTROL_MODE_POSITION_VELOCITY_PD. + if (!PyArg_ParseTuple(args, "iiiddddd", &bodyIndex, &jointIndex, + &controlMode, &targetPosition, &targetVelocity, + &maxForce, &kp, &kd)) { + PyErr_SetString(SpamError, "Error parsing arguments"); + return NULL; + } + valid = 1; + } + if (valid) { + int numJoints; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + struct b3JointInfo info; - - -// Set the gravity of the world with (x, y, z) arguments -static PyObject * -pybullet_setGravity(PyObject* self, PyObject* args) -{ - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; + numJoints = b3GetNumJoints(sm, bodyIndex); + if ((jointIndex >= numJoints) || (jointIndex < 0)) { + PyErr_SetString(SpamError, "Joint index out-of-range."); + return NULL; } - { - float gravX=0; - float gravY=0; - float gravZ=-10; - int ret; - - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); - b3SharedMemoryStatusHandle statusHandle; - - if (!PyArg_ParseTuple(args, "fff", &gravX,&gravY,&gravZ)) - { - PyErr_SetString(SpamError, "setGravity expected (x,y,z) values."); - return NULL; - } - ret = b3PhysicsParamSetGravity(command, gravX,gravY, gravZ); - //ret = b3PhysicsParamSetTimeStep(command, timeStep); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - //ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); - } - - Py_INCREF(Py_None); - return Py_None; -} - -static PyObject * -pybullet_setTimeStep(PyObject* self, PyObject* args) -{ - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; + if ((controlMode != CONTROL_MODE_VELOCITY) && + (controlMode != CONTROL_MODE_TORQUE) && + (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) { + PyErr_SetString(SpamError, "Illegral control mode."); + return NULL; } - - { - double timeStep=0.001; - int ret; - - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); - b3SharedMemoryStatusHandle statusHandle; - - if (!PyArg_ParseTuple(args, "d", &timeStep)) - { - PyErr_SetString(SpamError, "setTimeStep expected a single value (double)."); - return NULL; - } - ret = b3PhysicsParamSetTimeStep(command, timeStep); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - //ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); - } - + + commandHandle = b3JointControlCommandInit2(sm, bodyIndex, controlMode); + + b3GetJointInfo(sm, bodyIndex, jointIndex, &info); + + switch (controlMode) { + case CONTROL_MODE_VELOCITY: { + b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, + targetVelocity); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); + break; + } + + case CONTROL_MODE_TORQUE: { + b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, + appliedForce); + break; + } + + case CONTROL_MODE_POSITION_VELOCITY_PD: { + b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, + targetPosition); + b3JointControlSetKp(commandHandle, info.m_uIndex, kp); + b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, + targetVelocity); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); + break; + } + default: {} + }; + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + Py_INCREF(Py_None); return Py_None; + } + PyErr_SetString(SpamError, "Error parsing arguments in setJointControl."); + return NULL; } +static PyObject* pybullet_setRealTimeSimulation(PyObject* self, + PyObject* args) { + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + { + int enableRealTimeSimulation = 0; + int ret; + + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; + + if (!PyArg_ParseTuple(args, "i", &enableRealTimeSimulation)) { + PyErr_SetString( + SpamError, + "setRealTimeSimulation expected a single value (integer)."); + return NULL; + } + ret = + b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); + } + + Py_INCREF(Py_None); + return Py_None; +} + +// Set the gravity of the world with (x, y, z) arguments +static PyObject* pybullet_setGravity(PyObject* self, PyObject* args) { + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + double gravX = 0.0; + double gravY = 0.0; + double gravZ = -10.0; + int ret; + + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; + + if (!PyArg_ParseTuple(args, "ddd", &gravX, &gravY, &gravZ)) { + PyErr_SetString(SpamError, "setGravity expected (x,y,z) values."); + return NULL; + } + ret = b3PhysicsParamSetGravity(command, gravX, gravY, gravZ); + // ret = b3PhysicsParamSetTimeStep(command, timeStep); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); + } + + Py_INCREF(Py_None); + return Py_None; +} + +static PyObject* pybullet_setTimeStep(PyObject* self, PyObject* args) { + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + double timeStep = 0.001; + int ret; + + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; + + if (!PyArg_ParseTuple(args, "d", &timeStep)) { + PyErr_SetString(SpamError, + "setTimeStep expected a single value (double)."); + return NULL; + } + ret = b3PhysicsParamSetTimeStep(command, timeStep); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); + } + + Py_INCREF(Py_None); + return Py_None; +} // Internal function used to get the base position and orientation // Orientation is returned in quaternions -static int pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double basePosition[3],double baseOrientation[3]) -{ - basePosition[0] = 0.; - basePosition[1] = 0.; - basePosition[2] = 0.; - - baseOrientation[0] = 0.; - baseOrientation[1] = 0.; - baseOrientation[2] = 0.; - baseOrientation[3] = 1.; +static int pybullet_internalGetBasePositionAndOrientation( + int bodyIndex, double basePosition[3], double baseOrientation[3]) { + basePosition[0] = 0.; + basePosition[1] = 0.; + basePosition[2] = 0.; - { - - { - b3SharedMemoryCommandHandle cmd_handle = - b3RequestActualStateCommandInit(sm, bodyIndex); - b3SharedMemoryStatusHandle status_handle = - b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + baseOrientation[0] = 0.; + baseOrientation[1] = 0.; + baseOrientation[2] = 0.; + baseOrientation[3] = 1.; - const int status_type = b3GetStatusType(status_handle); - const double* actualStateQ; - // const double* jointReactionForces[]; - int i; + { + { + b3SharedMemoryCommandHandle cmd_handle = + b3RequestActualStateCommandInit(sm, bodyIndex); + b3SharedMemoryStatusHandle status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); - if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) - { - PyErr_SetString(SpamError, "getBasePositionAndOrientation failed."); - return 0; - } - - b3GetStatusActualState(status_handle, 0/* body_unique_id */, - 0/* num_degree_of_freedom_q */, - 0/* num_degree_of_freedom_u */, 0 /*root_local_inertial_frame*/, - &actualStateQ , 0 /* actual_state_q_dot */, - 0 /* joint_reaction_forces */); + const int status_type = b3GetStatusType(status_handle); + const double* actualStateQ; + // const double* jointReactionForces[]; + int i; - // printf("joint reaction forces="); - // for (i=0; i < (sizeof(jointReactionForces)/sizeof(double)); i++) { - // printf("%f ", jointReactionForces[i]); - // } - //now, position x,y,z = actualStateQ[0],actualStateQ[1],actualStateQ[2] - //and orientation x,y,z,w = actualStateQ[3],actualStateQ[4],actualStateQ[5],actualStateQ[6] - basePosition[0] = actualStateQ[0]; - basePosition[1] = actualStateQ[1]; - basePosition[2] = actualStateQ[2]; - - baseOrientation[0] = actualStateQ[3]; - baseOrientation[1] = actualStateQ[4]; - baseOrientation[2] = actualStateQ[5]; - baseOrientation[3] = actualStateQ[6]; - - } - } - return 1; + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { + PyErr_SetString(SpamError, "getBasePositionAndOrientation failed."); + return 0; + } + + b3GetStatusActualState( + status_handle, 0 /* body_unique_id */, + 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */, + 0 /*root_local_inertial_frame*/, &actualStateQ, + 0 /* actual_state_q_dot */, 0 /* joint_reaction_forces */); + + // printf("joint reaction forces="); + // for (i=0; i < (sizeof(jointReactionForces)/sizeof(double)); i++) { + // printf("%f ", jointReactionForces[i]); + // } + // now, position x,y,z = actualStateQ[0],actualStateQ[1],actualStateQ[2] + // and orientation x,y,z,w = + // actualStateQ[3],actualStateQ[4],actualStateQ[5],actualStateQ[6] + basePosition[0] = actualStateQ[0]; + basePosition[1] = actualStateQ[1]; + basePosition[2] = actualStateQ[2]; + + baseOrientation[0] = actualStateQ[3]; + baseOrientation[1] = actualStateQ[4]; + baseOrientation[2] = actualStateQ[5]; + baseOrientation[3] = actualStateQ[6]; + } + } + return 1; } // Get the positions (x,y,z) and orientation (x,y,z,w) in quaternion // values for the base link of your object // Object is retrieved based on body index, which is the order // the object was loaded into the simulation (0-based) -static PyObject * -pybullet_getBasePositionAndOrientation(PyObject* self, PyObject* args) -{ - int bodyIndex = -1; - double basePosition[3]; - double baseOrientation[4]; - PyObject *pylistPos; - PyObject *pylistOrientation; +static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self, + PyObject* args) { + int bodyIndex = -1; + double basePosition[3]; + double baseOrientation[4]; + PyObject* pylistPos; + PyObject* pylistOrientation; - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - if (!PyArg_ParseTuple(args, "i", &bodyIndex )) - { - PyErr_SetString(SpamError, "Expected a body index (integer)."); - return NULL; - } + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } - if (0==pybullet_internalGetBasePositionAndOrientation(bodyIndex, basePosition, baseOrientation)) - { - PyErr_SetString(SpamError, "GetBasePositionAndOrientation failed (#joints/links exceeds maximum?)."); - return NULL; - } - - { - - PyObject *item; - int i; - int num=3; - pylistPos = PyTuple_New(num); - for (i = 0; i < num; i++) - { - item = PyFloat_FromDouble(basePosition[i]); - PyTuple_SetItem(pylistPos, i, item); - } - - } + if (!PyArg_ParseTuple(args, "i", &bodyIndex)) { + PyErr_SetString(SpamError, "Expected a body index (integer)."); + return NULL; + } - { - - PyObject *item; - int i; - int num=4; - pylistOrientation = PyTuple_New(num); - for (i = 0; i < num; i++) - { - item = PyFloat_FromDouble(baseOrientation[i]); - PyTuple_SetItem(pylistOrientation, i, item); - } - + if (0 == pybullet_internalGetBasePositionAndOrientation( + bodyIndex, basePosition, baseOrientation)) { + PyErr_SetString(SpamError, + "GetBasePositionAndOrientation failed (#joints/links " + "exceeds maximum?)."); + return NULL; + } + + { + PyObject* item; + int i; + int num = 3; + pylistPos = PyTuple_New(num); + for (i = 0; i < num; i++) { + item = PyFloat_FromDouble(basePosition[i]); + PyTuple_SetItem(pylistPos, i, item); } - - { - PyObject *pylist; - pylist = PyTuple_New(2); - PyTuple_SetItem(pylist,0,pylistPos); - PyTuple_SetItem(pylist,1,pylistOrientation); - return pylist; + } + + { + PyObject* item; + int i; + int num = 4; + pylistOrientation = PyTuple_New(num); + for (i = 0; i < num; i++) { + item = PyFloat_FromDouble(baseOrientation[i]); + PyTuple_SetItem(pylistOrientation, i, item); } - + } + + { + PyObject* pylist; + pylist = PyTuple_New(2); + PyTuple_SetItem(pylist, 0, pylistPos); + PyTuple_SetItem(pylist, 1, pylistOrientation); + return pylist; + } } // Return the number of joints in an object based on // body index; body index is based on order of sequence // the object is loaded into simulation -static PyObject * -pybullet_getNumJoints(PyObject* self, PyObject* args) -{ - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } +static PyObject* pybullet_getNumJoints(PyObject* self, PyObject* args) { + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } - { - int bodyIndex = -1; - int numJoints=0; - if (!PyArg_ParseTuple(args, "i", &bodyIndex )) - { - PyErr_SetString(SpamError, "Expected a body index (integer)."); - return NULL; - } - numJoints = b3GetNumJoints(sm,bodyIndex); + { + int bodyIndex = -1; + int numJoints = 0; + if (!PyArg_ParseTuple(args, "i", &bodyIndex)) { + PyErr_SetString(SpamError, "Expected a body index (integer)."); + return NULL; + } + numJoints = b3GetNumJoints(sm, bodyIndex); #if PY_MAJOR_VERSION >= 3 - return PyLong_FromLong(numJoints); + return PyLong_FromLong(numJoints); #else - return PyInt_FromLong(numJoints); + return PyInt_FromLong(numJoints); #endif - } + } } // Initalize all joint positions given a list of values -static PyObject* -pybullet_resetJointState(PyObject* self, PyObject* args) -{ - int size; - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - - size= PySequence_Size(args); - - if (size==3) - { - int bodyIndex; - int jointIndex; - double targetValue; - - if (PyArg_ParseTuple(args, "iid", &bodyIndex, &jointIndex, &targetValue)) - { - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int numJoints; - - numJoints = b3GetNumJoints(sm,bodyIndex); - if ((jointIndex >= numJoints) || (jointIndex < 0)) - { - PyErr_SetString(SpamError, "Joint index out-of-range."); - return NULL; - } - - commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); - - b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, targetValue); - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - Py_INCREF(Py_None); - return Py_None; - } - - } - PyErr_SetString(SpamError, "error in resetJointState."); +static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args) { + int size; + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; -} + } -// Reset the position and orientation of the base/root link, position [x,y,z] and orientation quaternion [x,y,z,w] -static PyObject* -pybullet_resetBasePositionAndOrientation(PyObject* self, PyObject* args) -{ - int size; - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); + size = PySequence_Size(args); + + if (size == 3) { + int bodyIndex; + int jointIndex; + double targetValue; + + if (PyArg_ParseTuple(args, "iid", &bodyIndex, &jointIndex, &targetValue)) { + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int numJoints; + + numJoints = b3GetNumJoints(sm, bodyIndex); + if ((jointIndex >= numJoints) || (jointIndex < 0)) { + PyErr_SetString(SpamError, "Joint index out-of-range."); return NULL; - } - - - size= PySequence_Size(args); - - if (size==3) - { - int bodyIndex; - PyObject* posObj; - PyObject* ornObj; - double pos[3]; - double orn[4];//as a quaternion - - if (PyArg_ParseTuple(args, "iOO", &bodyIndex, &posObj, &ornObj)) - { - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - - { - PyObject* seq; - int len,i; - seq = PySequence_Fast(posObj, "expected a sequence"); - len = PySequence_Size(posObj); - if (len==3) - { - for (i = 0; i < 3; i++) - { - pos[i] = pybullet_internalGetFloatFromSequence(seq,i); - } - } else - { - PyErr_SetString(SpamError, "position needs a 3 coordinates [x,y,z]."); - Py_DECREF(seq); - return NULL; - } - Py_DECREF(seq); - } + } - { - PyObject* seq; - int len,i; - seq = PySequence_Fast(ornObj, "expected a sequence"); - len = PySequence_Size(ornObj); - if (len==4) - { - for (i = 0; i < 4; i++) - { - orn[i] = pybullet_internalGetFloatFromSequence(seq,i); - } - } else - { - PyErr_SetString(SpamError, "orientation needs a 4 coordinates, quaternion [x,y,z,w]."); - Py_DECREF(seq); - return NULL; - } - Py_DECREF(seq); - } - - commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); - - b3CreatePoseCommandSetBasePosition( commandHandle, pos[0],pos[1],pos[2]); - b3CreatePoseCommandSetBaseOrientation( commandHandle, orn[0],orn[1],orn[2],orn[3]); + commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - Py_INCREF(Py_None); - return Py_None; - } - + b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, + targetValue); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + Py_INCREF(Py_None); + return Py_None; } - PyErr_SetString(SpamError, "error in resetJointState."); - return NULL; + } + PyErr_SetString(SpamError, "error in resetJointState."); + return NULL; } +// Reset the position and orientation of the base/root link, position [x,y,z] +// and orientation quaternion [x,y,z,w] +static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self, + PyObject* args) { + int size; + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + size = PySequence_Size(args); + + if (size == 3) { + int bodyIndex; + PyObject* posObj; + PyObject* ornObj; + double pos[3]; + double orn[4]; // as a quaternion + + if (PyArg_ParseTuple(args, "iOO", &bodyIndex, &posObj, &ornObj)) { + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + + { + PyObject* seq; + int len, i; + seq = PySequence_Fast(posObj, "expected a sequence"); + len = PySequence_Size(posObj); + if (len == 3) { + for (i = 0; i < 3; i++) { + pos[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + } else { + PyErr_SetString(SpamError, "position needs a 3 coordinates [x,y,z]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } + + { + PyObject* seq; + int len, i; + seq = PySequence_Fast(ornObj, "expected a sequence"); + len = PySequence_Size(ornObj); + if (len == 4) { + for (i = 0; i < 4; i++) { + orn[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + } else { + PyErr_SetString( + SpamError, + "orientation needs a 4 coordinates, quaternion [x,y,z,w]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } + + commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); + + b3CreatePoseCommandSetBasePosition(commandHandle, pos[0], pos[1], pos[2]); + b3CreatePoseCommandSetBaseOrientation(commandHandle, orn[0], orn[1], + orn[2], orn[3]); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + Py_INCREF(Py_None); + return Py_None; + } + } + PyErr_SetString(SpamError, "error in resetJointState."); + return NULL; +} // Get the a single joint info for a specific bodyIndex // @@ -892,83 +774,66 @@ pybullet_resetBasePositionAndOrientation(PyObject* self, PyObject* args) // index, name, type, q-index, u-index, // flags, joint damping, joint friction // -// The format of the returned list is +// The format of the returned list is // [int, str, int, int, int, int, float, float] // // TODO(hellojas): get joint positions for a body -static PyObject* -pybullet_getJointInfo(PyObject* self, PyObject* args) -{ - PyObject *pyListJointInfo; - +static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args) { + PyObject* pyListJointInfo; + struct b3JointInfo info; - + int bodyIndex = -1; int jointIndex = -1; - int jointInfoSize = 8; //size of struct b3JointInfo - - int size= PySequence_Size(args); + int jointInfoSize = 8; // size of struct b3JointInfo - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } + int size = PySequence_Size(args); + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } - - if (size==2) // get body index and joint index + if (size == 2) // get body index and joint index { - if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) - { - + if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) { // printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex); - pyListJointInfo = PyTuple_New(jointInfoSize); + pyListJointInfo = PyTuple_New(jointInfoSize); - if (b3GetJointInfo(sm, bodyIndex, jointIndex, &info)) - { - - // printf("Joint%d %s, type %d, at q-index %d and u-index %d\n", - // info.m_jointIndex, - // info.m_jointName, - // info.m_jointType, - // info.m_qIndex, - // info.m_uIndex); - // printf(" flags=%d jointDamping=%f jointFriction=%f\n", - // info.m_flags, - // info.m_jointDamping, - // info.m_jointFriction); - PyTuple_SetItem(pyListJointInfo, 0, - PyInt_FromLong(info.m_jointIndex)); - PyTuple_SetItem(pyListJointInfo, 1, - PyString_FromString(info.m_jointName)); - PyTuple_SetItem(pyListJointInfo, 2, - PyInt_FromLong(info.m_jointType)); - PyTuple_SetItem(pyListJointInfo, 3, - PyInt_FromLong(info.m_qIndex)); - PyTuple_SetItem(pyListJointInfo, 4, - PyInt_FromLong(info.m_uIndex)); - PyTuple_SetItem(pyListJointInfo, 5, - PyInt_FromLong(info.m_flags)); - PyTuple_SetItem(pyListJointInfo, 6, - PyFloat_FromDouble(info.m_jointDamping)); - PyTuple_SetItem(pyListJointInfo, 7, - PyFloat_FromDouble(info.m_jointFriction)); - return pyListJointInfo; - } - else - { - PyErr_SetString(SpamError, "GetJointInfo failed."); - return NULL; - } + if (b3GetJointInfo(sm, bodyIndex, jointIndex, &info)) { + // printf("Joint%d %s, type %d, at q-index %d and u-index %d\n", + // info.m_jointIndex, + // info.m_jointName, + // info.m_jointType, + // info.m_qIndex, + // info.m_uIndex); + // printf(" flags=%d jointDamping=%f jointFriction=%f\n", + // info.m_flags, + // info.m_jointDamping, + // info.m_jointFriction); + PyTuple_SetItem(pyListJointInfo, 0, PyInt_FromLong(info.m_jointIndex)); + PyTuple_SetItem(pyListJointInfo, 1, + PyString_FromString(info.m_jointName)); + PyTuple_SetItem(pyListJointInfo, 2, PyInt_FromLong(info.m_jointType)); + PyTuple_SetItem(pyListJointInfo, 3, PyInt_FromLong(info.m_qIndex)); + PyTuple_SetItem(pyListJointInfo, 4, PyInt_FromLong(info.m_uIndex)); + PyTuple_SetItem(pyListJointInfo, 5, PyInt_FromLong(info.m_flags)); + PyTuple_SetItem(pyListJointInfo, 6, + PyFloat_FromDouble(info.m_jointDamping)); + PyTuple_SetItem(pyListJointInfo, 7, + PyFloat_FromDouble(info.m_jointFriction)); + return pyListJointInfo; + } else { + PyErr_SetString(SpamError, "GetJointInfo failed."); + return NULL; + } } } - - Py_INCREF(Py_None); - return Py_None; -} + Py_INCREF(Py_None); + return Py_None; +} // Returns the state of a specific joint in a given bodyIndex // @@ -980,111 +845,99 @@ pybullet_getJointInfo(PyObject* self, PyObject* args) // position, velocity, force torque (6 values), and motor torque // The returned pylist is an array of [float, float, float[6], float] -// TODO(hellojas): check accuracy of position and velocity +// TODO(hellojas): check accuracy of position and velocity // TODO(hellojas): check force torque values -static PyObject* -pybullet_getJointState(PyObject* self, PyObject* args) -{ - PyObject *pyListJointForceTorque; - PyObject *pyListJointState; - PyObject *item; - +static PyObject* pybullet_getJointState(PyObject* self, PyObject* args) { + PyObject* pyListJointForceTorque; + PyObject* pyListJointState; + PyObject* item; + struct b3JointInfo info; struct b3JointSensorState sensorState; - + int bodyIndex = -1; int jointIndex = -1; - int sensorStateSize = 4; // size of struct b3JointSensorState - int forceTorqueSize = 6; // size of force torque list from b3JointSensorState + int sensorStateSize = 4; // size of struct b3JointSensorState + int forceTorqueSize = 6; // size of force torque list from b3JointSensorState int i, j; - - - int size= PySequence_Size(args); - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } + int size = PySequence_Size(args); - if (size==2) // get body index and joint index + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (size == 2) // get body index and joint index { - if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) - { - int status_type = 0; - b3SharedMemoryCommandHandle cmd_handle = + if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) { + int status_type = 0; + b3SharedMemoryCommandHandle cmd_handle = b3RequestActualStateCommandInit(sm, bodyIndex); b3SharedMemoryStatusHandle status_handle = - b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); - - status_type = b3GetStatusType(status_handle); - if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) - { - PyErr_SetString(SpamError, "getBasePositionAndOrientation failed."); - return NULL; - } + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); - pyListJointState = PyTuple_New(sensorStateSize); - pyListJointForceTorque = PyTuple_New(forceTorqueSize); + status_type = b3GetStatusType(status_handle); + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { + PyErr_SetString(SpamError, "getBasePositionAndOrientation failed."); + return NULL; + } - - b3GetJointState(sm, status_handle, jointIndex, &sensorState); - - PyTuple_SetItem(pyListJointState, 0, - PyFloat_FromDouble(sensorState.m_jointPosition)); - PyTuple_SetItem(pyListJointState, 1, - PyFloat_FromDouble(sensorState.m_jointVelocity)); - - for (j = 0; j < forceTorqueSize; j++) { - PyTuple_SetItem(pyListJointForceTorque, j, - PyFloat_FromDouble(sensorState.m_jointForceTorque[j])); + pyListJointState = PyTuple_New(sensorStateSize); + pyListJointForceTorque = PyTuple_New(forceTorqueSize); + + b3GetJointState(sm, status_handle, jointIndex, &sensorState); + + PyTuple_SetItem(pyListJointState, 0, + PyFloat_FromDouble(sensorState.m_jointPosition)); + PyTuple_SetItem(pyListJointState, 1, + PyFloat_FromDouble(sensorState.m_jointVelocity)); + + for (j = 0; j < forceTorqueSize; j++) { + PyTuple_SetItem(pyListJointForceTorque, j, + PyFloat_FromDouble(sensorState.m_jointForceTorque[j])); + } + + PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque); + + PyTuple_SetItem(pyListJointState, 3, + PyFloat_FromDouble(sensorState.m_jointMotorTorque)); + + return pyListJointState; } - - PyTuple_SetItem(pyListJointState, 2, - pyListJointForceTorque); - - PyTuple_SetItem(pyListJointState, 3, - PyFloat_FromDouble(sensorState.m_jointMotorTorque)); - - return pyListJointState; - } - } else - { - PyErr_SetString(SpamError, "getJointState expects 2 arguments (objectUniqueId and joint index)."); - return NULL; + } else { + PyErr_SetString( + SpamError, + "getJointState expects 2 arguments (objectUniqueId and joint index)."); + return NULL; } - - Py_INCREF(Py_None); - return Py_None; + + Py_INCREF(Py_None); + return Py_None; } - - // internal function to set a float matrix[16] // used to initialize camera position with // a view and projection matrix in renderImage() // // // Args: // matrix - float[16] which will be set by values from objMat -static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) -{ - int i, len; - PyObject* seq; +static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) { + int i, len; + PyObject* seq; - seq = PySequence_Fast(objMat, "expected a sequence"); - len = PySequence_Size(objMat); - if (len==16) - { - for (i = 0; i < len; i++) - { - matrix[i] = pybullet_internalGetFloatFromSequence(seq,i); - } - Py_DECREF(seq); - return 1; + seq = PySequence_Fast(objMat, "expected a sequence"); + len = PySequence_Size(objMat); + if (len == 16) { + for (i = 0; i < len; i++) { + matrix[i] = pybullet_internalGetFloatFromSequence(seq, i); } Py_DECREF(seq); - return 0; + return 1; + } + Py_DECREF(seq); + return 0; } // internal function to set a float vector[3] @@ -1092,138 +945,140 @@ static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) // a view and projection matrix in renderImage() // // // Args: -// matrix - float[16] which will be set by values from objMat -static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) -{ - int i, len; - PyObject* seq; +// vector - float[3] which will be set by values from objMat +static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) { + int i, len; + PyObject* seq; - seq = PySequence_Fast(objMat, "expected a sequence"); - len = PySequence_Size(objMat); - if (len==3) - { - for (i = 0; i < len; i++) - { - vector[i] = pybullet_internalGetFloatFromSequence(seq,i); - } - Py_DECREF(seq); - return 1; + seq = PySequence_Fast(objMat, "expected a sequence"); + len = PySequence_Size(objMat); + if (len == 3) { + for (i = 0; i < len; i++) { + vector[i] = pybullet_internalGetFloatFromSequence(seq, i); } Py_DECREF(seq); - return 0; + return 1; + } + Py_DECREF(seq); + return 0; } +static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) { + int size = PySequence_Size(args); + int objectUniqueIdA = -1; + int objectUniqueIdB = -1; + b3SharedMemoryCommandHandle commandHandle; + struct b3ContactInformation contactPointData; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int i; + PyObject* pyResultList = 0; + if (size == 1) { + if (!PyArg_ParseTuple(args, "i", &objectUniqueIdA)) { + PyErr_SetString(SpamError, "Error parsing object unique id"); + return NULL; + } + } + if (size == 2) { + if (!PyArg_ParseTuple(args, "ii", &objectUniqueIdA, &objectUniqueIdB)) { + PyErr_SetString(SpamError, "Error parsing object unique id"); + return NULL; + } + } -static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) -{ - int size= PySequence_Size(args); - int objectUniqueIdA = -1; - int objectUniqueIdB = -1; - b3SharedMemoryCommandHandle commandHandle; - struct b3ContactInformation contactPointData; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - int i; - - PyObject* pyResultList=0; - - if (size==1) - { - if (!PyArg_ParseTuple(args, "i", &objectUniqueIdA)) - { - PyErr_SetString(SpamError, "Error parsing object unique id"); - return NULL; - } + commandHandle = b3InitRequestContactPointInformation(sm); + b3SetContactFilterBodyA(commandHandle, objectUniqueIdA); + b3SetContactFilterBodyB(commandHandle, objectUniqueIdB); + b3SubmitClientCommand(sm, commandHandle); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) { + /* + 0 int m_contactFlags; + 1 int m_bodyUniqueIdA; + 2 int m_bodyUniqueIdB; + 3 int m_linkIndexA; + 4 int m_linkIndexB; + 5-6-7 double m_positionOnAInWS[3];//contact point location on object A, + in world space coordinates + 8-9-10 double m_positionOnBInWS[3];//contact point location on object + A, in world space coordinates + 11-12-13 double m_contactNormalOnBInWS[3];//the separating contact + normal, pointing from object B towards object A + 14 double m_contactDistance;//negative number is penetration, positive + is distance. + + 15 double m_normalForce; + */ + + b3GetContactPointInformation(sm, &contactPointData); + pyResultList = PyTuple_New(contactPointData.m_numContactPoints); + for (i = 0; i < contactPointData.m_numContactPoints; i++) { + PyObject* contactObList = PyTuple_New(16); // see above 16 fields + PyObject* item; + item = + PyInt_FromLong(contactPointData.m_contactPointData[i].m_contactFlags); + PyTuple_SetItem(contactObList, 0, item); + item = PyInt_FromLong( + contactPointData.m_contactPointData[i].m_bodyUniqueIdA); + PyTuple_SetItem(contactObList, 1, item); + item = PyInt_FromLong( + contactPointData.m_contactPointData[i].m_bodyUniqueIdB); + PyTuple_SetItem(contactObList, 2, item); + item = + PyInt_FromLong(contactPointData.m_contactPointData[i].m_linkIndexA); + PyTuple_SetItem(contactObList, 3, item); + item = + PyInt_FromLong(contactPointData.m_contactPointData[i].m_linkIndexB); + PyTuple_SetItem(contactObList, 4, item); + item = PyFloat_FromDouble( + contactPointData.m_contactPointData[i].m_positionOnAInWS[0]); + PyTuple_SetItem(contactObList, 5, item); + item = PyFloat_FromDouble( + contactPointData.m_contactPointData[i].m_positionOnAInWS[1]); + PyTuple_SetItem(contactObList, 6, item); + item = PyFloat_FromDouble( + contactPointData.m_contactPointData[i].m_positionOnAInWS[2]); + PyTuple_SetItem(contactObList, 7, item); + + item = PyFloat_FromDouble( + contactPointData.m_contactPointData[i].m_positionOnBInWS[0]); + PyTuple_SetItem(contactObList, 8, item); + item = PyFloat_FromDouble( + contactPointData.m_contactPointData[i].m_positionOnBInWS[1]); + PyTuple_SetItem(contactObList, 9, item); + item = PyFloat_FromDouble( + contactPointData.m_contactPointData[i].m_positionOnBInWS[2]); + PyTuple_SetItem(contactObList, 10, item); + + item = PyFloat_FromDouble( + contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[0]); + PyTuple_SetItem(contactObList, 11, item); + item = PyFloat_FromDouble( + contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[1]); + PyTuple_SetItem(contactObList, 12, item); + item = PyFloat_FromDouble( + contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[2]); + PyTuple_SetItem(contactObList, 13, item); + + item = PyFloat_FromDouble( + contactPointData.m_contactPointData[i].m_contactDistance); + PyTuple_SetItem(contactObList, 14, item); + item = PyFloat_FromDouble( + contactPointData.m_contactPointData[i].m_normalForce); + PyTuple_SetItem(contactObList, 15, item); + + PyTuple_SetItem(pyResultList, i, contactObList); } - if (size==2) - { - if (!PyArg_ParseTuple(args, "ii", &objectUniqueIdA,&objectUniqueIdB)) - { - PyErr_SetString(SpamError, "Error parsing object unique id"); - return NULL; - } - } - - commandHandle = b3InitRequestContactPointInformation(sm); - b3SetContactFilterBodyA(commandHandle,objectUniqueIdA); - b3SetContactFilterBodyB(commandHandle,objectUniqueIdB); - b3SubmitClientCommand(sm, commandHandle); - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - if (statusType==CMD_CONTACT_POINT_INFORMATION_COMPLETED) - { - - /* - 0 int m_contactFlags; - 1 int m_bodyUniqueIdA; - 2 int m_bodyUniqueIdB; - 3 int m_linkIndexA; - 4 int m_linkIndexB; - 5-6-7 double m_positionOnAInWS[3];//contact point location on object A, in world space coordinates - 8-9-10 double m_positionOnBInWS[3];//contact point location on object A, in world space coordinates - 11-12-13 double m_contactNormalOnBInWS[3];//the separating contact normal, pointing from object B towards object A - 14 double m_contactDistance;//negative number is penetration, positive is distance. - - 15 double m_normalForce; - */ - - b3GetContactPointInformation(sm, &contactPointData); - pyResultList = PyTuple_New(contactPointData.m_numContactPoints); - for (i=0;i euler yaw/pitch/roll convention from URDF/SDF, see Gazebo +/// https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc +static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, + PyObject* args) { + double squ; + double sqx; + double sqy; + double sqz; -static PyObject* pybullet_getQuaternionFromEuler(PyObject* self, PyObject* args) -{ + double quat[4]; + + PyObject* quatObj; + + if (PyArg_ParseTuple(args, "O", &quatObj)) { + PyObject* seq; + int len, i; + seq = PySequence_Fast(quatObj, "expected a sequence"); + len = PySequence_Size(quatObj); + if (len == 4) { + for (i = 0; i < 4; i++) { + quat[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + } else { + PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } else { + PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w]."); + return NULL; + } + + { double rpy[3]; - - PyObject* eulerObj; - - if (PyArg_ParseTuple(args, "O", &eulerObj)) + double sarg; + sqx = quat[0] * quat[0]; + sqy = quat[1] * quat[1]; + sqz = quat[2] * quat[2]; + squ = quat[3] * quat[3]; + rpy[0] = atan2(2 * (quat[1] * quat[2] + quat[3] * quat[0]), + squ - sqx - sqy + sqz); + sarg = -2 * (quat[0] * quat[2] - quat[3] * quat[1]); + rpy[1] = sarg <= -1.0 ? -0.5 * 3.141592538 + : (sarg >= 1.0 ? 0.5 * 3.141592538 : asin(sarg)); + rpy[2] = atan2(2 * (quat[0] * quat[1] + quat[3] * quat[2]), + squ + sqx - sqy - sqz); { - PyObject* seq; - int len,i; - seq = PySequence_Fast(eulerObj, "expected a sequence"); - len = PySequence_Size(eulerObj); - if (len==3) - { - for (i = 0; i < 3; i++) - { - rpy[i] = pybullet_internalGetFloatFromSequence(seq,i); - } - } else - { - PyErr_SetString(SpamError, "Euler angles need a 3 coordinates [roll, pitch, yaw]."); - Py_DECREF(seq); - return NULL; + PyObject* pylist; + int i; + pylist = PyTuple_New(3); + for (i = 0; i < 3; i++) + PyTuple_SetItem(pylist, i, PyFloat_FromDouble(rpy[i])); + return pylist; + } + } + Py_INCREF(Py_None); + return Py_None; +} + +/// Given an object id, joint positions, joint velocities and joint +/// accelerations, +/// compute the joint forces using Inverse Dynamics +static PyObject* pybullet_calculateInverseDynamics(PyObject* self, + PyObject* args) { + int size; + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + size = PySequence_Size(args); + if (size == 4) { + int bodyIndex; + PyObject* objPositionsQ; + PyObject* objVelocitiesQdot; + PyObject* objAccelerations; + + if (PyArg_ParseTuple(args, "iOOO", &bodyIndex, &objPositionsQ, + &objVelocitiesQdot, &objAccelerations)) { + int szObPos = PySequence_Size(objPositionsQ); + int szObVel = PySequence_Size(objVelocitiesQdot); + int szObAcc = PySequence_Size(objAccelerations); + int numJoints = b3GetNumJoints(sm, bodyIndex); + if (numJoints && (szObPos == numJoints) && (szObVel == numJoints) && + (szObAcc == numJoints)) { + int szInBytes = sizeof(double) * numJoints; + int i; + PyObject* pylist = 0; + double* jointPositionsQ = (double*)malloc(szInBytes); + double* jointVelocitiesQdot = (double*)malloc(szInBytes); + double* jointAccelerations = (double*)malloc(szInBytes); + double* jointForcesOutput = (double*)malloc(szInBytes); + + for (i = 0; i < numJoints; i++) { + jointPositionsQ[i] = + pybullet_internalGetFloatFromSequence(objPositionsQ, i); + jointVelocitiesQdot[i] = + pybullet_internalGetFloatFromSequence(objVelocitiesQdot, i); + jointAccelerations[i] = + pybullet_internalGetFloatFromSequence(objAccelerations, i); } - Py_DECREF(seq); - } else - { - PyErr_SetString(SpamError, "Euler angles need a 3 coordinates [roll, pitch, yaw]."); + + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle commandHandle = + b3CalculateInverseDynamicsCommandInit( + sm, bodyIndex, jointPositionsQ, jointVelocitiesQdot, + jointAccelerations); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) { + int bodyUniqueId; + int dofCount; + + b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId, + &dofCount, 0); + + if (dofCount) { + b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0, + jointForcesOutput); + { + { + int i; + pylist = PyTuple_New(dofCount); + for (i = 0; i < dofCount; i++) + PyTuple_SetItem(pylist, i, + PyFloat_FromDouble(jointForcesOutput[i])); + } + } + } + + } else { + PyErr_SetString(SpamError, + "Internal error in calculateInverseDynamics"); + } + } + free(jointPositionsQ); + free(jointVelocitiesQdot); + free(jointAccelerations); + free(jointForcesOutput); + if (pylist) return pylist; + } else { + PyErr_SetString(SpamError, + "calculateInverseDynamics numJoints needs to be " + "positive and [joint positions], [joint velocities], " + "[joint accelerations] need to match the number of " + "joints."); return NULL; + } + + } else { + PyErr_SetString(SpamError, + "calculateInverseDynamics expects 4 arguments, body " + "index, [joint positions], [joint velocities], [joint " + "accelerations]."); + return NULL; } - - { - double phi, the, psi; - double roll = rpy[0]; - double pitch = rpy[1]; - double yaw = rpy[2]; - phi = roll / 2.0; - the = pitch / 2.0; - psi = yaw / 2.0; - { - double quat[4] = { - sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi), - cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi), - cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi), - cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi)}; - - //normalize the quaternion - double len = sqrt(quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3]); - quat[0] /= len; - quat[1] /= len; - quat[2] /= len; - quat[3] /= len; - { - PyObject *pylist; - int i; - pylist = PyTuple_New(4); - for (i=0;i<4;i++) - PyTuple_SetItem(pylist,i,PyFloat_FromDouble(quat[i])); - return pylist; - } - } - } - Py_INCREF(Py_None); - return Py_None; - + } else { + PyErr_SetString(SpamError, + "calculateInverseDynamics expects 4 arguments, body index, " + "[joint positions], [joint velocities], [joint " + "accelerations]."); + return NULL; + } + Py_INCREF(Py_None); + return Py_None; } -///quaternion <-> euler yaw/pitch/roll convention from URDF/SDF, see Gazebo -///https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc -static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, PyObject* args) -{ - - double squ; - double sqx; - double sqy; - double sqz; - - double quat[4]; - - PyObject* quatObj; - - if (PyArg_ParseTuple(args, "O", &quatObj)) - { - PyObject* seq; - int len,i; - seq = PySequence_Fast(quatObj, "expected a sequence"); - len = PySequence_Size(quatObj); - if (len==4) - { - for (i = 0; i < 4; i++) - { - quat[i] = pybullet_internalGetFloatFromSequence(seq,i); - } - } else - { - PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w]."); - Py_DECREF(seq); - return NULL; - } - Py_DECREF(seq); - } else - { - PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w]."); - return NULL; - } - - { - double rpy[3]; - double sarg; - sqx = quat[0] * quat[0]; - sqy = quat[1] * quat[1]; - sqz = quat[2] * quat[2]; - squ = quat[3] * quat[3]; - rpy[0] = atan2(2 * (quat[1]*quat[2] + quat[3]*quat[0]), squ - sqx - sqy + sqz); - sarg = -2 * (quat[0]*quat[2] - quat[3] * quat[1]); - rpy[1] = sarg <= -1.0 ? -0.5*3.141592538 : (sarg >= 1.0 ? 0.5*3.141592538 : asin(sarg)); - rpy[2] = atan2(2 * (quat[0]*quat[1] + quat[3]*quat[2]), squ + sqx - sqy - sqz); - { - PyObject *pylist; - int i; - pylist = PyTuple_New(3); - for (i=0;i<3;i++) - PyTuple_SetItem(pylist,i,PyFloat_FromDouble(rpy[i])); - return pylist; - } - } - Py_INCREF(Py_None); - return Py_None; -} - -///Given an object id, joint positions, joint velocities and joint accelerations, -///compute the joint forces using Inverse Dynamics -static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* args) -{ - int size; - if (0 == sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - size = PySequence_Size(args); - if (size==4) - { - - int bodyIndex; - PyObject* objPositionsQ; - PyObject* objVelocitiesQdot; - PyObject* objAccelerations; - - if (PyArg_ParseTuple(args, "iOOO", &bodyIndex, &objPositionsQ, &objVelocitiesQdot, &objAccelerations)) - { - int szObPos = PySequence_Size(objPositionsQ); - int szObVel = PySequence_Size(objVelocitiesQdot); - int szObAcc = PySequence_Size(objAccelerations); - int numJoints = b3GetNumJoints(sm, bodyIndex); - if (numJoints && (szObPos == numJoints) && (szObVel == numJoints) && (szObAcc == numJoints)) - { - int szInBytes = sizeof(double)*numJoints; - int i; - PyObject* pylist = 0; - double* jointPositionsQ = (double*)malloc(szInBytes); - double* jointVelocitiesQdot = (double*)malloc(szInBytes); - double* jointAccelerations = (double*)malloc(szInBytes); - double* jointForcesOutput = (double*)malloc(szInBytes); - - for (i = 0; i < numJoints; i++) - { - jointPositionsQ[i] = pybullet_internalGetFloatFromSequence(objPositionsQ, i); - jointVelocitiesQdot[i] = pybullet_internalGetFloatFromSequence(objVelocitiesQdot, i); - jointAccelerations[i] = pybullet_internalGetFloatFromSequence(objAccelerations, i); - } - - { - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(sm, - bodyIndex, jointPositionsQ, jointVelocitiesQdot, jointAccelerations); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - - statusType = b3GetStatusType(statusHandle); - - if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) - { - int bodyUniqueId; - int dofCount; - - b3GetStatusInverseDynamicsJointForces(statusHandle, - &bodyUniqueId, - &dofCount, - 0); - - if (dofCount) - { - b3GetStatusInverseDynamicsJointForces(statusHandle, - 0, - 0, - jointForcesOutput); - { - { - - int i; - pylist = PyTuple_New(dofCount); - for (i = 0; i= 3 - static struct PyModuleDef moduledef = { - PyModuleDef_HEAD_INIT, - "pybullet", /* m_name */ - "Python bindings for Bullet Physics Robotics API (also known as Shared Memory API)", /* m_doc */ - -1, /* m_size */ - SpamMethods, /* m_methods */ - NULL, /* m_reload */ - NULL, /* m_traverse */ - NULL, /* m_clear */ - NULL, /* m_free */ - }; +static struct PyModuleDef moduledef = { + PyModuleDef_HEAD_INIT, "pybullet", /* m_name */ + "Python bindings for Bullet Physics Robotics API (also known as Shared " + "Memory API)", /* m_doc */ + -1, /* m_size */ + SpamMethods, /* m_methods */ + NULL, /* m_reload */ + NULL, /* m_traverse */ + NULL, /* m_clear */ + NULL, /* m_free */ +}; #endif PyMODINIT_FUNC @@ -1954,39 +1789,37 @@ initpybullet(void) #endif { - PyObject *m; + PyObject* m; #if PY_MAJOR_VERSION >= 3 - m = PyModule_Create(&moduledef); + m = PyModule_Create(&moduledef); #else - m = Py_InitModule3("pybullet", - SpamMethods, "Python bindings for Bullet"); + m = Py_InitModule3("pybullet", SpamMethods, "Python bindings for Bullet"); #endif #if PY_MAJOR_VERSION >= 3 - if (m == NULL) - return m; + if (m == NULL) return m; #else - if (m == NULL) - return; + if (m == NULL) return; #endif - - - PyModule_AddIntConstant (m, "SHARED_MEMORY", eCONNECT_SHARED_MEMORY); // user read - PyModule_AddIntConstant (m, "DIRECT", eCONNECT_DIRECT); // user read - PyModule_AddIntConstant (m, "GUI", eCONNECT_GUI); // user read - - PyModule_AddIntConstant (m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE); - PyModule_AddIntConstant (m, "VELOCITY_CONTROL", CONTROL_MODE_VELOCITY); // user read - PyModule_AddIntConstant (m, "POSITION_CONTROL", CONTROL_MODE_POSITION_VELOCITY_PD); // user read - - PyModule_AddIntConstant (m, "LINK_FRAME", EF_LINK_FRAME); - PyModule_AddIntConstant (m, "WORLD_FRAME", EF_WORLD_FRAME); - - SpamError = PyErr_NewException("pybullet.error", NULL, NULL); - Py_INCREF(SpamError); - PyModule_AddObject(m, "error", SpamError); + + PyModule_AddIntConstant(m, "SHARED_MEMORY", + eCONNECT_SHARED_MEMORY); // user read + PyModule_AddIntConstant(m, "DIRECT", eCONNECT_DIRECT); // user read + PyModule_AddIntConstant(m, "GUI", eCONNECT_GUI); // user read + + PyModule_AddIntConstant(m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE); + PyModule_AddIntConstant(m, "VELOCITY_CONTROL", + CONTROL_MODE_VELOCITY); // user read + PyModule_AddIntConstant(m, "POSITION_CONTROL", + CONTROL_MODE_POSITION_VELOCITY_PD); // user read + + PyModule_AddIntConstant(m, "LINK_FRAME", EF_LINK_FRAME); + PyModule_AddIntConstant(m, "WORLD_FRAME", EF_WORLD_FRAME); + + SpamError = PyErr_NewException("pybullet.error", NULL, NULL); + Py_INCREF(SpamError); + PyModule_AddObject(m, "error", SpamError); #if PY_MAJOR_VERSION >= 3 - return m; + return m; #endif } - From 32eccdff6149aed60d508d5ff450510bac527d54 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Thu, 8 Sep 2016 15:15:58 -0700 Subject: [PATCH 10/14] Create project file for BussIK inverse kinematics library (premake, cmake) URDF/SDF: add a flag to force concave mesh collisiofor static objects. VR: support teleporting using buttong, allow multiple controllers to be used, fast wireframe rendering, Turn off warnings about deprecated C routine in btScalar.h/b3Scalar.h Add a dummy return to stop a warning Expose defaultContactERP in shared memory api/pybullet. First start to expose IK in shared memory api/pybullet (not working yet) --- build3/premake4.lua | 1 + data/cube_small.urdf | 1 + data/kiva_shelf/model.sdf | 18 +- data/multibody.bullet | Bin 14660 -> 14488 bytes data/samurai.urdf | 29 +++ examples/CMakeLists.txt | 2 +- .../CommonInterfaces/CommonCameraInterface.h | 1 + .../CommonInterfaces/CommonRenderInterface.h | 3 +- examples/ExampleBrowser/CMakeLists.txt | 23 +- examples/ExampleBrowser/OpenGLGuiHelper.cpp | 42 +++- examples/ExampleBrowser/OpenGLGuiHelper.h | 2 + examples/ExampleBrowser/premake4.lua | 4 +- .../ImportURDFDemo/BulletUrdfImporter.cpp | 33 ++- .../Importers/ImportURDFDemo/UrdfParser.cpp | 3 + .../Importers/ImportURDFDemo/UrdfParser.h | 11 + .../OpenGLWindow/GLInstancingRenderer.cpp | 15 +- examples/OpenGLWindow/GLInstancingRenderer.h | 6 +- examples/OpenGLWindow/SimpleCamera.cpp | 21 +- examples/OpenGLWindow/SimpleCamera.h | 2 + .../RoboticsLearning/KukaGraspExample.cpp | 38 ++- examples/RoboticsLearning/b3RobotSimAPI.cpp | 27 +++ examples/RoboticsLearning/b3RobotSimAPI.h | 32 +++ .../IKTrajectoryHelper.cpp | 15 +- .../IKTrajectoryHelper.h | 2 + examples/SharedMemory/PhysicsClientC_API.cpp | 73 +++++- examples/SharedMemory/PhysicsClientC_API.h | 12 + .../PhysicsServerCommandProcessor.cpp | 120 +++++++++- .../SharedMemory/PhysicsServerExample.cpp | 217 ++++++++++++------ examples/SharedMemory/SharedMemoryCommands.h | 20 ++ examples/SharedMemory/SharedMemoryPublic.h | 2 + examples/SharedMemory/premake4.lua | 10 +- .../StandaloneMain/hellovr_opengl_main.cpp | 64 +++++- examples/ThirdPartyLibs/BussIK/CMakeLists.txt | 10 + examples/ThirdPartyLibs/BussIK/Misc.cpp | 2 +- examples/ThirdPartyLibs/BussIK/premake4.lua | 11 + examples/pybullet/CMakeLists.txt | 4 +- examples/pybullet/premake4.lua | 4 +- examples/pybullet/pybullet.c | 34 +++ src/Bullet3Common/b3Scalar.h | 2 +- .../details/MultiBodyTreeImpl.cpp | 1 + src/LinearMath/btScalar.h | 2 +- test/SharedMemory/CMakeLists.txt | 4 +- test/SharedMemory/premake4.lua | 12 +- 43 files changed, 791 insertions(+), 144 deletions(-) create mode 100644 data/samurai.urdf rename examples/{RoboticsLearning => SharedMemory}/IKTrajectoryHelper.cpp (93%) rename examples/{RoboticsLearning => SharedMemory}/IKTrajectoryHelper.h (92%) create mode 100644 examples/ThirdPartyLibs/BussIK/CMakeLists.txt create mode 100644 examples/ThirdPartyLibs/BussIK/premake4.lua diff --git a/build3/premake4.lua b/build3/premake4.lua index 3f92bba58..f0de611fd 100644 --- a/build3/premake4.lua +++ b/build3/premake4.lua @@ -249,6 +249,7 @@ end include "../examples/InverseDynamics" include "../examples/ExtendedTutorials" include "../examples/SharedMemory" + include "../examples/ThirdPartyLibs/BussIK" include "../examples/MultiThreading" if _OPTIONS["lua"] then diff --git a/data/cube_small.urdf b/data/cube_small.urdf index f6e271ee3..a0e3cc609 100644 --- a/data/cube_small.urdf +++ b/data/cube_small.urdf @@ -5,6 +5,7 @@ + diff --git a/data/kiva_shelf/model.sdf b/data/kiva_shelf/model.sdf index 3e506a9b0..138f88d6d 100644 --- a/data/kiva_shelf/model.sdf +++ b/data/kiva_shelf/model.sdf @@ -2,18 +2,18 @@ 1 - 0 2 0 0 0 0 + 0 1 0 0 0 0 0.0 .0 1.2045 0 0 0 - 76.26 + 0 - 47 - -0.003456 - 0.001474 - 13.075 - -0.014439 - 47 + 0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 @@ -30,7 +30,7 @@ - + 0 0 0 1.5707 0 0 diff --git a/data/multibody.bullet b/data/multibody.bullet index 50ed04b49b35aa23c1e25653a29892410d61977c..19dacc389d0aa171f6386162662e30c365b555b6 100644 GIT binary patch delta 1357 zcmZ`(O-NKx6h8O)W}F{`#J>!&Ck}Gi%XWsX|bI$kOd*7Y+CO#P} zc3g71PxsxtI()pP*>&0N3Z%CIYcL*mt}#+aIk&MzRJ z;+@Pgc1YK`^z~wNk?y;3o##nLv8475Vk2uQH!kwy0NSJS|no#PCOQIbV?uj^$G} zp-jZ8!2g9sJHnd99ym8`%}c1U=gcOzUbZ&`mfLB4*5wE4Uf5WFU)5?f%x4M|H!Y4D z^JEU^MfSp?kt$3;O~*=WrU^v;J1d~d@n^1qp#%q`@yM`$&>v1FtV&-Xh*bDSXvpW^$@!E-lvJoT zl&kb6gORY}4f~Yq{vCUI3!=T5*OSA1g%8lzY9F=B=MBj>gg*( zS{hF|#*1~;{GOrJw0aW2bn@8!8q=I6JDjzfeJDnBN-H^ zf>Y1}ZoCd4qH90qU9iT0O~MtJg8+PC(*=fHFRBA6CBef!zyup*@8AbynTI8%ap|?3 kg%I<=1Zw!u$AZ5e?9YIwhy~yUTa#|H4ESd6G%OYU25wKDUH||9 delta 1513 zcmai!TS!!45Xa}7J-d5cS6frb+rwI`T`bK&(_q)!wzAktE7^nui!R3?S}Q6@ajED6 z1)jc#qzhO?*@Y~6NUGiJLPc3f64jga5Z03rsP&M|oHJ`uP~Y<3`F=BV=9}~FuO+#~*hEQwrewv&z~bszcJsP|-v3vyO#%Vo2AdeO8Ulzo!9 zG+|F3@eSrL`qm)vOXjtSxxK`nn74g<`H}bs^Y%|)PY|ben)MIcj?H6U}lr+fP zze_tqtXwg>8W}4ZA)d!v_x(@>UR6$sLP@9=4y^VE>Hzyu(MOtt;vWik05lqQaZ5?p zV_C*vu2*^_i_opL;)RkdBG%*9G9x9)HbvZ59EP{dmX<;fuBoc5rf335q}WpF1nBm( zL`B$^Kk0XZ1Ko~)CD4Q?8VOVaD|k=Z)5>4<2E90^zVqnAy$=m6TIFA!fp6f%J!uAN z;tQF*G|8zVX!na*ic62Fiy{v&Yr^^;L&QuT&x?S_b%ex1Rn4Wt;{*BU;pTfzxXrVw zvmQ##Vm@Q)0V2#xm|?ydUlETp*T&;?#klI@apq0L)Yi-jY$<*(@d5a=HS>nYTl3Vf zD!db=vacTwMW1!$`I_jZaAjm?j*PYbwix$P+>8Q@4mo{$dwZjsL+`xgI~@u|4|e6n zPBZT=EsPm$B@1jTu(GmP#s85cxP8EJ)opdbTyTlvg3Q(iXag6N;U^8MaAp;pPX);h z)yNI_?S_TM$Aw%C1CmQ>hjq{m8jMOeEvgX%j8LTrXwo3GOGl*vP^I^hCPm~nxkq^q k`y>t8@gxm0a9A|V*5Em4PAQBf2jrttAB1neEqLtw1+|S=ga7~l diff --git a/data/samurai.urdf b/data/samurai.urdf new file mode 100644 index 000000000..f15d47fb6 --- /dev/null +++ b/data/samurai.urdf @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 8a974c55b..47728c244 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -1,6 +1,6 @@ SUBDIRS( HelloWorld BasicDemo ) IF(BUILD_BULLET3) - SUBDIRS( ExampleBrowser ThirdPartyLibs/Gwen OpenGLWindow ) + SUBDIRS( ExampleBrowser ThirdPartyLibs/Gwen ThirdPartyLibs/BussIK OpenGLWindow ) ENDIF() IF(BUILD_PYBULLET) SUBDIRS(pybullet) diff --git a/examples/CommonInterfaces/CommonCameraInterface.h b/examples/CommonInterfaces/CommonCameraInterface.h index 9c1740788..1e9fa2f25 100644 --- a/examples/CommonInterfaces/CommonCameraInterface.h +++ b/examples/CommonInterfaces/CommonCameraInterface.h @@ -9,6 +9,7 @@ struct CommonCameraInterface virtual void setVRCamera(const float viewMat[16], const float projectionMatrix[16])=0; virtual void disableVRCamera()=0; virtual bool isVRCamera() const =0; + virtual void setVRCameraOffsetTransform(const float offset[16])=0; virtual void getCameraTargetPosition(float pos[3]) const = 0; virtual void getCameraPosition(float pos[3]) const = 0; diff --git a/examples/CommonInterfaces/CommonRenderInterface.h b/examples/CommonInterfaces/CommonRenderInterface.h index 0ac09975f..888607dd9 100644 --- a/examples/CommonInterfaces/CommonRenderInterface.h +++ b/examples/CommonInterfaces/CommonRenderInterface.h @@ -27,8 +27,9 @@ struct CommonRenderInterface virtual CommonCameraInterface* getActiveCamera()=0; virtual void setActiveCamera(CommonCameraInterface* cam)=0; - virtual void renderScene()=0; + virtual void renderScene()=0; + virtual void renderSceneInternal(int renderMode=B3_DEFAULT_RENDERMODE){}; virtual int getScreenWidth() = 0; virtual int getScreenHeight() = 0; diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 717d57f28..32f4c18f6 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -47,20 +47,20 @@ IF (BUILD_SHARED_LIBS) IF (WIN32) TARGET_LINK_LIBRARIES( BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils - BulletInverseDynamics LinearMath OpenGLWindow gwen + BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} ) ELSE(WIN32) IF(APPLE) TARGET_LINK_LIBRARIES( BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils - BulletInverseDynamics LinearMath OpenGLWindow gwen + BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK ${COCOA} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} ) ELSE(APPLE) TARGET_LINK_LIBRARIES( BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils - BulletInverseDynamics LinearMath OpenGLWindow gwen + BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK pthread dl ) ENDIF(APPLE) @@ -81,7 +81,7 @@ INCLUDE_DIRECTORIES( LINK_LIBRARIES( - BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen + BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK ) IF (WIN32) @@ -129,6 +129,8 @@ SET(BulletExampleBrowser_SRCS ../TinyRenderer/TinyRenderer.cpp ../SharedMemory/TinyRendererVisualShapeConverter.cpp ../SharedMemory/TinyRendererVisualShapeConverter.h + ../SharedMemory/IKTrajectoryHelper.cpp + ../SharedMemory/IKTrajectoryHelper.h ../RenderingExamples/TinyRendererSetup.cpp ../SharedMemory/PhysicsServer.cpp ../SharedMemory/PhysicsClientSharedMemory.cpp @@ -218,8 +220,6 @@ SET(BulletExampleBrowser_SRCS ../RoboticsLearning/R2D2GraspExample.h ../RoboticsLearning/KukaGraspExample.cpp ../RoboticsLearning/KukaGraspExample.h - ../RoboticsLearning/IKTrajectoryHelper.cpp - ../RoboticsLearning/IKTrajectoryHelper.h ../RenderingExamples/CoordinateSystemDemo.cpp ../RenderingExamples/CoordinateSystemDemo.h ../RenderingExamples/RaytracerSetup.cpp @@ -307,17 +307,6 @@ SET(BulletExampleBrowser_SRCS ../ThirdPartyLibs/stb_image/stb_image.cpp ../ThirdPartyLibs/stb_image/stb_image.h - ../ThirdPartyLibs/BussIK/Jacobian.cpp - ../ThirdPartyLibs/BussIK/Tree.cpp - ../ThirdPartyLibs/BussIK/Node.cpp - ../ThirdPartyLibs/BussIK/LinearR2.cpp - ../ThirdPartyLibs/BussIK/LinearR3.cpp - ../ThirdPartyLibs/BussIK/LinearR4.cpp - ../ThirdPartyLibs/BussIK/MatrixRmn.cpp - ../ThirdPartyLibs/BussIK/VectorRn.cpp - ../ThirdPartyLibs/BussIK/Misc.cpp - - ../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp ../ThirdPartyLibs/tinyxml/tinystr.cpp ../ThirdPartyLibs/tinyxml/tinyxml.cpp diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index 8001b11e3..ecd1ed45d 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -146,11 +146,26 @@ struct OpenGLGuiHelperInternalData struct CommonGraphicsApp* m_glApp; class MyDebugDrawer* m_debugDraw; GL_ShapeDrawer* m_gl2ShapeDrawer; - + bool m_vrMode; + int m_vrSkipShadowPass; + btAlignedObjectArray m_rgbaPixelBuffer1; btAlignedObjectArray m_depthBuffer1; + + OpenGLGuiHelperInternalData() + :m_vrMode(false), + m_vrSkipShadowPass(0) + { + } + }; +void OpenGLGuiHelper::setVRMode(bool vrMode) +{ + m_data->m_vrMode = vrMode; + m_data->m_vrSkipShadowPass = 0; +} + OpenGLGuiHelper::OpenGLGuiHelper(CommonGraphicsApp* glApp, bool useOpenGL2) @@ -269,6 +284,10 @@ void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* colli } void OpenGLGuiHelper::syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld) { + //in VR mode, we skip the synchronization for the second eye + if (m_data->m_vrMode && m_data->m_vrSkipShadowPass==1) + return; + int numCollisionObjects = rbWorld->getNumCollisionObjects(); for (int i = 0; im_glApp->m_renderer->renderScene(); + if (m_data->m_vrMode) + { + //in VR, we skip the shadow generation for the second eye + + if (m_data->m_vrSkipShadowPass>=1) + { + m_data->m_glApp->m_renderer->renderSceneInternal(B3_USE_SHADOWMAP_RENDERMODE); + m_data->m_vrSkipShadowPass=0; + + } else + { + m_data->m_glApp->m_renderer->renderScene(); + m_data->m_vrSkipShadowPass++; + } + } else + { + m_data->m_glApp->m_renderer->renderScene(); + } + //backwards compatible OpenGL2 rendering if (m_data->m_gl2ShapeDrawer && rbWorld) diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h index 2fac0313e..eaa72a7be 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.h +++ b/examples/ExampleBrowser/OpenGLGuiHelper.h @@ -56,6 +56,8 @@ struct OpenGLGuiHelper : public GUIHelperInterface virtual void drawText3D( const char* txt, float posX, float posY, float posZ, float size); void renderInternalGl2(int pass, const btDiscreteDynamicsWorld* dynamicsWorld); + + void setVRMode(bool vrMode); }; #endif //OPENGL_GUI_HELPER_H diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 2010c744e..5bd7eec42 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -10,7 +10,7 @@ project "App_BulletExampleBrowser" initOpenCL("clew") end - links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"} + links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK", "Bullet3Common"} initOpenGL() initGlew() @@ -55,6 +55,8 @@ project "App_BulletExampleBrowser" "../TinyRenderer/our_gl.cpp", "../TinyRenderer/TinyRenderer.cpp", "../RenderingExamples/TinyRendererSetup.cpp", + "../SharedMemory/IKTrajectoryHelper.cpp", + "../SharedMemory/IKTrajectoryHelper.h", "../SharedMemory/PhysicsClientC_API.cpp", "../SharedMemory/PhysicsClientC_API.h", "../SharedMemory/PhysicsServerExample.cpp", diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 992840850..85186069f 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -577,13 +577,32 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co glmesh->m_vertices->at(i).xyzw[1]*collision->m_geometry.m_meshScale[1], glmesh->m_vertices->at(i).xyzw[2]*collision->m_geometry.m_meshScale[2])); } - //btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex)); - btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); - //cylZShape->initializePolyhedralFeatures(); - //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.); - //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); - cylZShape->setMargin(0.001); - shape = cylZShape; + + if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH) + { + btTriangleMesh* meshInterface = new btTriangleMesh(); + for (int i=0;im_numIndices/3;i++) + { + float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw; + float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw; + float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw; + meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]), + btVector3(v1[0],v1[1],v1[2]), + btVector3(v2[0],v2[1],v2[2])); + } + + btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true); + shape = trimesh; + } else + { + //btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex)); + btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); + //cylZShape->initializePolyhedralFeatures(); + //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.); + //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); + cylZShape->setMargin(0.001); + shape = cylZShape; + } } else { b3Warning("issue extracting mesh from STL file %s\n", fullPath); diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 3fee80465..20e6a920f 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -489,6 +489,9 @@ bool UrdfParser::parseCollision(UrdfCollision& collision, TiXmlElement* config, if (name_char) collision.m_name = name_char; + const char *concave_char = config->Attribute("concave"); + if (concave_char) + collision.m_flags |= URDF_FORCE_CONCAVE_TRIMESH; return true; } diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 25f7e3e38..0ed995f33 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -75,11 +75,22 @@ struct UrdfVisual UrdfMaterial m_localMaterial; }; +enum UrdfCollisionFlags +{ + URDF_FORCE_CONCAVE_TRIMESH=1, +}; + + struct UrdfCollision { btTransform m_linkLocalFrame; UrdfGeometry m_geometry; std::string m_name; + int m_flags; + UrdfCollision() + :m_flags(0) + { + } }; struct UrdfJoint; diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index 5107ed758..1a8ee2063 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp @@ -17,7 +17,7 @@ subject to the following restrictions: ///todo: make this configurable in the gui bool useShadowMap = true;// true;//false;//true; -int shadowMapWidth= 2048;//8192; +int shadowMapWidth= 2048; int shadowMapHeight= 2048; float shadowMapWorldSize=5; @@ -329,6 +329,19 @@ void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* positi } +void GLInstancingRenderer::readSingleInstanceTransformFromCPU(int srcIndex, float* position, float* orientation) +{ + b3Assert(srcIndexm_totalNumInstances); + b3Assert(srcIndex>=0); + position[0] = m_data->m_instance_positions_ptr[srcIndex*4+0]; + position[1] = m_data->m_instance_positions_ptr[srcIndex*4+1]; + position[2] = m_data->m_instance_positions_ptr[srcIndex*4+2]; + + orientation[0] = m_data->m_instance_quaternion_ptr[srcIndex*4+0]; + orientation[1] = m_data->m_instance_quaternion_ptr[srcIndex*4+1]; + orientation[2] = m_data->m_instance_quaternion_ptr[srcIndex*4+2]; + orientation[3] = m_data->m_instance_quaternion_ptr[srcIndex*4+3]; +} void GLInstancingRenderer::writeSingleInstanceColorToCPU(double* color, int srcIndex) { m_data->m_instance_colors_ptr[srcIndex*4+0]=float(color[0]); diff --git a/examples/OpenGLWindow/GLInstancingRenderer.h b/examples/OpenGLWindow/GLInstancingRenderer.h index 135f44133..9fae49db5 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.h +++ b/examples/OpenGLWindow/GLInstancingRenderer.h @@ -41,7 +41,7 @@ class GLInstancingRenderer : public CommonRenderInterface int m_upAxis; bool m_enableBlend; - void renderSceneInternal(int renderMode=B3_DEFAULT_RENDERMODE); + @@ -52,6 +52,7 @@ public: virtual void init(); virtual void renderScene(); + virtual void renderSceneInternal(int renderMode=B3_DEFAULT_RENDERMODE); void InitShaders(); void CleanupShaders(); @@ -73,6 +74,7 @@ public: void writeTransforms(); + virtual void writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex); virtual void writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex) { @@ -90,6 +92,8 @@ public: } + virtual void readSingleInstanceTransformFromCPU(int srcIndex, float* position, float* orientation); + virtual void writeSingleInstanceTransformToGPU(float* position, float* orientation, int srcIndex); virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex); diff --git a/examples/OpenGLWindow/SimpleCamera.cpp b/examples/OpenGLWindow/SimpleCamera.cpp index 43729b236..a78f4de25 100644 --- a/examples/OpenGLWindow/SimpleCamera.cpp +++ b/examples/OpenGLWindow/SimpleCamera.cpp @@ -3,6 +3,8 @@ #include "Bullet3Common/b3Vector3.h" #include "Bullet3Common/b3Quaternion.h" #include "Bullet3Common/b3Matrix3x3.h" +#include "Bullet3Common/b3Transform.h" + struct SimpleCameraInternalData { @@ -19,6 +21,9 @@ struct SimpleCameraInternalData m_frustumZFar(1000), m_enableVR(false) { + b3Transform tr; + tr.setIdentity(); + tr.getOpenGLMatrix(m_offsetTransformVR); } b3Vector3 m_cameraTargetPosition; float m_cameraDistance; @@ -37,6 +42,7 @@ struct SimpleCameraInternalData bool m_enableVR; float m_viewMatrixVR[16]; float m_projectionMatrixVR[16]; + float m_offsetTransformVR[16]; }; @@ -244,13 +250,26 @@ void SimpleCamera::getCameraProjectionMatrix(float projectionMatrix[16]) const b3CreateFrustum(-m_data->m_aspect * m_data->m_frustumZNear, m_data->m_aspect * m_data->m_frustumZNear, -m_data->m_frustumZNear,m_data->m_frustumZNear, m_data->m_frustumZNear, m_data->m_frustumZFar,projectionMatrix); } } +void SimpleCamera::setVRCameraOffsetTransform(const float offset[16]) +{ + for (int i=0;i<16;i++) + { + m_data->m_offsetTransformVR[i] = offset[i]; + } +} void SimpleCamera::getCameraViewMatrix(float viewMatrix[16]) const { if (m_data->m_enableVR) { for (int i=0;i<16;i++) { - viewMatrix[i] = m_data->m_viewMatrixVR[i]; + b3Transform tr; + tr.setFromOpenGLMatrix(m_data->m_viewMatrixVR); + b3Transform shift=b3Transform::getIdentity(); + shift.setFromOpenGLMatrix(m_data->m_offsetTransformVR); + tr = tr*shift; + tr.getOpenGLMatrix(viewMatrix); + //viewMatrix[i] = m_data->m_viewMatrixVR[i]; } } else { diff --git a/examples/OpenGLWindow/SimpleCamera.h b/examples/OpenGLWindow/SimpleCamera.h index 5a057baf0..7221f01a6 100644 --- a/examples/OpenGLWindow/SimpleCamera.h +++ b/examples/OpenGLWindow/SimpleCamera.h @@ -15,7 +15,9 @@ struct SimpleCamera : public CommonCameraInterface virtual void getCameraViewMatrix(float m[16]) const; virtual void setVRCamera(const float viewMat[16], const float projectionMatrix[16]); + virtual void setVRCameraOffsetTransform(const float offset[16]); virtual void disableVRCamera(); + virtual bool isVRCamera() const; virtual void getCameraTargetPosition(float pos[3]) const; diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index 52cf68160..5eb89d916 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -1,6 +1,6 @@ #include "KukaGraspExample.h" -#include "IKTrajectoryHelper.h" +#include "../SharedMemory/IKTrajectoryHelper.h" #include "../CommonInterfaces/CommonGraphicsAppInterface.h" #include "Bullet3Common/b3Quaternion.h" @@ -138,8 +138,11 @@ public: } virtual void stepSimulation(float deltaTime) { - m_time+=deltaTime; - m_targetPos.setValue(0.5, 0, 0.5+0.4*b3Sin(3 * m_time)); + float dt = deltaTime; + btClamp(dt,0.0001f,0.01f); + + m_time+=dt; + m_targetPos.setValue(0.4-0.4*b3Cos( m_time), 0, 0.8+0.4*b3Cos( m_time)); @@ -158,11 +161,36 @@ public: q_current[i] = jointStates.m_actualStateQ[i+7]; } } + b3Vector3DoubleData dataOut; + m_targetPos.serializeDouble(dataOut); + + // m_robotSim.getJointInfo(m_kukaIndex,jointIndex,jointInfo); +/* + b3RobotSimInverseKinematicArgs ikargs; + b3RobotSimInverseKinematicsResults ikresults; + + + ikargs.m_bodyUniqueId = m_kukaIndex; + ikargs.m_currentJointPositions = q_current; + ikargs.m_numPositions = 7; + ikargs.m_endEffectorTargetPosition[0] = dataOut.m_floats[0]; + ikargs.m_endEffectorTargetPosition[1] = dataOut.m_floats[1]; + ikargs.m_endEffectorTargetPosition[2] = dataOut.m_floats[2]; + + + ikargs.m_endEffectorTargetOrientation[0] = 0; + ikargs.m_endEffectorTargetOrientation[1] = 0; + ikargs.m_endEffectorTargetOrientation[2] = 0; + ikargs.m_endEffectorTargetOrientation[3] = 1; + + if (m_robotSim.calculateInverseKinematics(ikargs,ikresults)) + { + } +*/ double q_new[7]; int ikMethod=IK2_SDLS; - b3Vector3DoubleData dataOut; - m_targetPos.serializeDouble(dataOut); + m_ikHelper.computeIK(dataOut.m_floats,q_current, numJoints, q_new, ikMethod); printf("q_new = %f,%f,%f,%f,%f,%f,%f\n", q_new[0],q_new[1],q_new[2], q_new[3],q_new[4],q_new[5],q_new[6]); diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index e7daae828..957e27d5c 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -469,6 +469,33 @@ void b3RobotSimAPI::setNumSimulationSubSteps(int numSubSteps) b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); } +/* +b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, + const double* jointPositionsQ, double targetPosition[3]); + +int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, + int* bodyUniqueId, + int* dofCount, + double* jointPositions); +*/ +bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKinematicArgs& args, struct b3RobotSimInverseKinematicsResults& results) +{ + + b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId, + args.m_currentJointPositions,args.m_endEffectorTargetPosition); + + b3SharedMemoryStatusHandle statusHandle; + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); + + bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle, + &results.m_bodyUniqueId, + &results.m_numPositions, + results.m_calculatedJointPositions); + + return result; +} + + b3RobotSimAPI::~b3RobotSimAPI() { delete m_data; diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index 142d406cf..16c379609 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -50,6 +50,7 @@ struct b3RobotSimLoadFileArgs } }; + struct b3RobotSimLoadFileResults { b3AlignedObjectArray m_uniqueObjectIds; @@ -81,6 +82,35 @@ struct b3JointMotorArgs } }; +enum b3InverseKinematicsFlags +{ + B3_HAS_IK_TARGET_ORIENTATION=1, +}; + +struct b3RobotSimInverseKinematicArgs +{ + int m_bodyUniqueId; + double* m_currentJointPositions; + int m_numPositions; + double m_endEffectorTargetPosition[3]; + double m_endEffectorTargetOrientation[3]; + int m_flags; + + b3RobotSimInverseKinematicArgs() + :m_bodyUniqueId(-1), + m_currentJointPositions(0), + m_numPositions(0), + m_flags(0) + { + } +}; + +struct b3RobotSimInverseKinematicsResults +{ + int m_bodyUniqueId; + double* m_calculatedJointPositions; + int m_numPositions; +}; class b3RobotSimAPI { @@ -113,6 +143,8 @@ public: void setNumSimulationSubSteps(int numSubSteps); + bool calculateInverseKinematics(const struct b3RobotSimInverseKinematicArgs& args, struct b3RobotSimInverseKinematicsResults& results); + void renderScene(); void debugDraw(int debugDrawMode); }; diff --git a/examples/RoboticsLearning/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp similarity index 93% rename from examples/RoboticsLearning/IKTrajectoryHelper.cpp rename to examples/SharedMemory/IKTrajectoryHelper.cpp index 2e97653f6..4954f6b10 100644 --- a/examples/RoboticsLearning/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -4,11 +4,11 @@ #include "BussIK/Jacobian.h" #include "BussIK/VectorRn.h" #include "Bullet3Common/b3AlignedObjectArray.h" +#include "BulletDynamics/Featherstone/btMultiBody.h" #define RADIAN(X) ((X)*RadiansToDegrees) - //use BussIK and Reflexxes to convert from Cartesian endeffector future target to //joint space positions at each real-time (simulation) step struct IKTrajectoryHelperInternalData @@ -36,6 +36,17 @@ IKTrajectoryHelper::~IKTrajectoryHelper() delete m_data; } +bool IKTrajectoryHelper::createFromMultiBody(class btMultiBody* mb) +{ + //todo: implement proper conversion. For now, we only 'detect' a likely KUKA iiwa and hardcode its creation + if (mb->getNumLinks()==7) + { + createKukaIIWA(); + return true; + } + + return false; +} void IKTrajectoryHelper::createKukaIIWA() { @@ -142,4 +153,4 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], q_new[i] = m_data->m_ikNodes[i]->GetTheta(); } return true; -} \ No newline at end of file +} diff --git a/examples/RoboticsLearning/IKTrajectoryHelper.h b/examples/SharedMemory/IKTrajectoryHelper.h similarity index 92% rename from examples/RoboticsLearning/IKTrajectoryHelper.h rename to examples/SharedMemory/IKTrajectoryHelper.h index 5b436057f..6bbd53815 100644 --- a/examples/RoboticsLearning/IKTrajectoryHelper.h +++ b/examples/SharedMemory/IKTrajectoryHelper.h @@ -22,6 +22,8 @@ public: ///todo: replace createKukaIIWA with a generic way of create an IK tree void createKukaIIWA(); + bool createFromMultiBody(class btMultiBody* mb); + bool computeIK(const double endEffectorTargetPosition[3], const double* q_old, int numQ, double* q_new, int ikMethod); diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index d57113e3e..e4f027695 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -163,6 +163,19 @@ int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int return 0; } + +int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + command->m_updateFlags |= SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP; + command->m_physSimParamArgs.m_defaultContactERP = defaultContactERP; + return 0; +} + + +int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP); + b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; @@ -1251,4 +1264,62 @@ int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandl return true; -} \ No newline at end of file +} + + +///compute the joint positions to move the end effector to a desired target using inverse kinematics +b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, + const double* jointPositionsQ, const double targetPosition[3]) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + + command->m_type = CMD_CALCULATE_INVERSE_KINEMATICS; + command->m_updateFlags = 0; + command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyIndex; + int numJoints = cl->getNumJoints(bodyIndex); + for (int i = 0; i < numJoints;i++) + { + command->m_calculateInverseKinematicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i]; + } + + command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0]; + command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1]; + command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2]; + + return (b3SharedMemoryCommandHandle)command; + +} + +int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, + int* bodyUniqueId, + int* dofCount, + double* jointPositions) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; + btAssert(status->m_type == CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED); + if (status->m_type != CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED) + return false; + + + if (dofCount) + { + *dofCount = status->m_inverseKinematicsResultArgs.m_dofCount; + } + if (bodyUniqueId) + { + *bodyUniqueId = status->m_inverseKinematicsResultArgs.m_bodyUniqueId; + } + if (jointPositions) + { + for (int i = 0; i < status->m_inverseKinematicsResultArgs.m_dofCount; i++) + { + jointPositions[i] = status->m_inverseKinematicsResultArgs.m_jointPositions[i]; + } + } + + return false; +} diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 4d6522576..60a647e9a 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -89,6 +89,7 @@ void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3Con b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient); int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz); int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep); +int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP); int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps); int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation); @@ -115,6 +116,17 @@ int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandl int* dofCount, double* jointForces); +///compute the joint positions to move the end effector to a desired target using inverse kinematics +b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, + const double* jointPositionsQ, const double targetPosition[3]); + +int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, + int* bodyUniqueId, + int* dofCount, + double* jointPositions); + + + b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 03357e59f..cb5a3b422 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -15,7 +15,7 @@ #include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h" #include "LinearMath/btHashMap.h" #include "BulletInverseDynamics/MultiBodyTree.hpp" - +#include "IKTrajectoryHelper.h" #include "btBulletDynamicsCommon.h" #include "LinearMath/btTransform.h" @@ -381,6 +381,8 @@ struct PhysicsServerCommandProcessorInternalData bool m_allowRealTimeSimulation; bool m_hasGround; + btMultiBodyFixedConstraint* m_gripperRigidbodyFixed; + CommandLogger* m_commandLogger; CommandLogPlayback* m_logPlayback; @@ -389,6 +391,7 @@ struct PhysicsServerCommandProcessorInternalData btScalar m_numSimulationSubSteps; btAlignedObjectArray m_multiBodyJointFeedbacks; btHashMap m_inverseDynamicsBodies; + btHashMap m_inverseKinematicsHelpers; @@ -428,7 +431,8 @@ struct PhysicsServerCommandProcessorInternalData PhysicsServerCommandProcessorInternalData() :m_hasGround(false), - m_allowRealTimeSimulation(false), + m_gripperRigidbodyFixed(0), + m_allowRealTimeSimulation(true), m_commandLogger(0), m_logPlayback(0), m_physicsDeltaTime(1./240.), @@ -556,6 +560,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0)); + m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.005; } void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies() @@ -1061,6 +1066,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm //no timestamp yet int timeStamp = 0; + + //catch uninitialized cases + serverStatusOut.m_type = CMD_INVALID_STATUS; //consume the command switch (clientCmd.m_type) @@ -1868,6 +1876,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps; } + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP) + { + m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = clientCmd.m_physSimParamArgs.m_defaultContactERP; + } + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; hasStatus = true; @@ -1961,6 +1974,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED; hasStatus = true; m_data->m_hasGround = false; + m_data->m_gripperRigidbodyFixed = 0; break; } case CMD_CREATE_RIGID_BODY: @@ -2441,9 +2455,50 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } } + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; hasStatus = true; break; } + case CMD_CALCULATE_INVERSE_KINEMATICS: + { + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_FAILED; + + InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId); + if (bodyHandle && bodyHandle->m_multiBody) + { + IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody); + IKTrajectoryHelper* ikHelperPtr = 0; + + + if (ikHelperPtrPtr) + { + ikHelperPtr = *ikHelperPtrPtr; + } + else + { + IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper; + if (tmpHelper->createFromMultiBody(bodyHandle->m_multiBody)) + { + m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, ikHelperPtr); + ikHelperPtr = tmpHelper; + } else + { + delete tmpHelper; + } + } + + if (ikHelperPtr) + { + + serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED; + } + } + hasStatus = true; + break; + } + default: { b3Error("Unknown command encountered"); @@ -2461,13 +2516,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm return hasStatus; } +static int skip=1; void PhysicsServerCommandProcessor::renderScene() { if (m_data->m_guiHelper) { m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld); - m_data->m_guiHelper->render(m_data->m_dynamicsWorld); } @@ -2485,6 +2540,7 @@ void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags) } } +btVector3 gLastPickPos(0,0,0); bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) { @@ -2498,6 +2554,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons { btVector3 pickPos = rayCallback.m_hitPointWorld; + gLastPickPos = pickPos; btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject); if (body) { @@ -2516,6 +2573,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons //very weak constraint for picking p2p->m_setting.m_tau = 0.001f; } + } else { btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject); @@ -2634,23 +2692,69 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName) m_data->m_logPlayback = pb; } +btVector3 gVRGripperPos(0,0,0); +btQuaternion gVRGripperOrn(0,0,0,1); + void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) { - if (m_data->m_allowRealTimeSimulation) + if (m_data->m_allowRealTimeSimulation && m_data->m_guiHelper) { + btAlignedObjectArray bufferServerToClient; + bufferServerToClient.resize(32768); + + if (!m_data->m_hasGround) { m_data->m_hasGround = true; int bodyId = 0; - btAlignedObjectArray bufferServerToClient; - bufferServerToClient.resize(32768); - + loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &bufferServerToClient[0], bufferServerToClient.size()); } - m_data->m_dynamicsWorld->stepSimulation(dtInSec,10,m_data->m_physicsDeltaTime); + if (0)//m_data->m_gripperRigidbodyFixed==0) + { + int bodyId = 0; + + loadUrdf("pr2_gripper.urdf",btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &bufferServerToClient[0], bufferServerToClient.size()); + + InteralBodyData* parentBody = m_data->getHandle(bodyId); + if (parentBody->m_multiBody) + { + parentBody->m_multiBody->setHasSelfCollision(1); + btVector3 pivotInParent(0,0,0); + btMatrix3x3 frameInParent; + frameInParent.setRotation(btQuaternion(0,0,0,1)); + + btVector3 pivotInChild(0,0,0); + btMatrix3x3 frameInChild; + frameInChild.setIdentity(); + + m_data->m_gripperRigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,-1,0,pivotInParent,pivotInChild,frameInParent,frameInChild); + m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(2.); + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; + world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed); + } + } + + if (m_data->m_gripperRigidbodyFixed) + { + m_data->m_gripperRigidbodyFixed->setFrameInB(btMatrix3x3(gVRGripperOrn)); + m_data->m_gripperRigidbodyFixed->setPivotInB(gVRGripperPos); + } + + int maxSteps = 3; + + int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec,maxSteps,m_data->m_physicsDeltaTime); + int droppedSteps = numSteps > maxSteps ? numSteps - maxSteps : 0; + static int skipReport = 0; + skipReport++; + if (skipReport>100) + { + skipReport = 0; + //printf("numSteps: %d (dropped %d), %f (internal step = %f)\n", numSteps, droppedSteps , dtInSec, m_data->m_physicsDeltaTime); + } } } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 73901c659..ad5fa0fd3 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -13,8 +13,11 @@ #include "../Utils/b3Clock.h" #include "../MultiThreading/b3ThreadSupportInterface.h" +#define MAX_VR_CONTROLLERS 4 - +extern btVector3 gLastPickPos; +btVector3 gVRTeleportPos(0,0,0); +bool gDebugRenderToggle = false; void MotionThreadFunc(void* userPtr,void* lsMemory); void* MotionlsMemoryFunc(); #define MAX_MOTION_NUM_THREADS 1 @@ -82,22 +85,27 @@ b3ThreadSupportInterface* createMotionThreadSupport(int numThreads) struct MotionArgs { MotionArgs() - :m_physicsServerPtr(0), - m_isPicking(false), - m_isDragging(false), - m_isReleasing(false) + :m_physicsServerPtr(0) { + for (int i=0;i m_positions; - btVector3 m_pos; - btQuaternion m_orn; - bool m_isPicking; - bool m_isDragging; - bool m_isReleasing; + btVector3 m_vrControllerPos[MAX_VR_CONTROLLERS]; + btQuaternion m_vrControllerOrn[MAX_VR_CONTROLLERS]; + bool m_isVrControllerPicking[MAX_VR_CONTROLLERS]; + bool m_isVrControllerDragging[MAX_VR_CONTROLLERS]; + bool m_isVrControllerReleasing[MAX_VR_CONTROLLERS]; + bool m_isVrControllerTeleporting[MAX_VR_CONTROLLERS]; }; @@ -108,6 +116,7 @@ struct MotionThreadLocalStorage int skip = 0; int skip1 = 0; +float clampedDeltaTime = 0.2; void MotionThreadFunc(void* userPtr,void* lsMemory) { @@ -127,12 +136,11 @@ void MotionThreadFunc(void* userPtr,void* lsMemory) args->m_cs->unlock(); + double deltaTimeInSeconds = 0; + do { -//todo(erwincoumans): do we want some sleep to reduce CPU resources in this thread? - - - double deltaTimeInSeconds = double(clock.getTimeMicroseconds())/1000000.; + deltaTimeInSeconds+= double(clock.getTimeMicroseconds())/1000000.; if (deltaTimeInSeconds<(1./5000.)) { @@ -150,40 +158,58 @@ void MotionThreadFunc(void* userPtr,void* lsMemory) //process special controller commands, such as //VR controller button press/release and controller motion - btVector3 from = args->m_pos; - btMatrix3x3 mat(args->m_orn); - btScalar pickDistance = 100.; - btVector3 toX = args->m_pos+mat.getColumn(0); - btVector3 toY = args->m_pos+mat.getColumn(1); - btVector3 toZ = args->m_pos+mat.getColumn(2)*pickDistance; - - - if (args->m_isPicking) + for (int c=0;cm_isPicking = false; - args->m_isDragging = true; - args->m_physicsServerPtr->pickBody(from,-toZ); - //printf("PICK!\n"); - } - - if (args->m_isDragging) - { - args->m_physicsServerPtr->movePickedBody(from,-toZ); - // printf("."); - } - if (args->m_isReleasing) - { - args->m_isDragging = false; - args->m_isReleasing = false; - args->m_physicsServerPtr->removePickingConstraint(); - //printf("Release pick\n"); + btVector3 from = args->m_vrControllerPos[c]; + btMatrix3x3 mat(args->m_vrControllerOrn[c]); + + btScalar pickDistance = 1000.; + btVector3 toX = from+mat.getColumn(0); + btVector3 toY = from+mat.getColumn(1); + btVector3 toZ = from+mat.getColumn(2)*pickDistance; + + if (args->m_isVrControllerTeleporting[c]) + { + args->m_isVrControllerTeleporting[c] = false; + args->m_physicsServerPtr->pickBody(from,-toZ); + args->m_physicsServerPtr->removePickingConstraint(); + } + + if (args->m_isVrControllerPicking[c]) + { + args->m_isVrControllerPicking[c] = false; + args->m_isVrControllerDragging[c] = true; + args->m_physicsServerPtr->pickBody(from,-toZ); + //printf("PICK!\n"); + } + + if (args->m_isVrControllerDragging[c]) + { + args->m_physicsServerPtr->movePickedBody(from,-toZ); + // printf("."); + } + + if (args->m_isVrControllerReleasing[c]) + { + args->m_isVrControllerDragging[c] = false; + args->m_isVrControllerReleasing[c] = false; + args->m_physicsServerPtr->removePickingConstraint(); + //printf("Release pick\n"); + } } //don't simulate over a huge timestep if we had some interruption (debugger breakpoint etc) - btClamp(deltaTimeInSeconds,0.,0.1); - args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds); + if (deltaTimeInSeconds>clampedDeltaTime) + { + deltaTimeInSeconds = clampedDeltaTime; + b3Warning("Clamp deltaTime from %f to %f",deltaTimeInSeconds, clampedDeltaTime); + } + clock.reset(); + args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds); + deltaTimeInSeconds = 0; + } args->m_physicsServerPtr->processClientCommands(); @@ -318,7 +344,10 @@ public: m_childGuiHelper->render(0); } - virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld){} + virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld) + { + m_childGuiHelper->createPhysicsDebugDrawer(rbWorld); + } virtual int registerTexture(const unsigned char* texels, int width, int height) { @@ -845,36 +874,52 @@ void PhysicsServerExample::stepSimulation(float deltaTime) } } +static float vrOffset[16]={1,0,0,0, + 0,1,0,0, + 0,0,1,0, + 0,0,0,0}; + void PhysicsServerExample::renderScene() { ///debug rendering //m_args[0].m_cs->lock(); + vrOffset[12]=-gVRTeleportPos[0]; + vrOffset[13]=-gVRTeleportPos[1]; + vrOffset[14]=-gVRTeleportPos[2]; + + this->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()-> + getActiveCamera()->setVRCameraOffsetTransform(vrOffset); + m_physicsServer.renderScene(); - if (m_args[0].m_isPicking || m_args[0].m_isDragging) + for (int i=0;igetAppInterface()->m_renderer->drawLine(from,toX,color,width); - color=btVector4(0,1,0,1); - m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toY,color,width); - color=btVector4(0,0,1,1); - m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toZ,color,width); + btVector4 color; + color=btVector4(1,0,0,1); + m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toX,color,width); + color=btVector4(0,1,0,1); + m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toY,color,width); + color=btVector4(0,0,1,1); + m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toZ,color,width); + } } + if (gDebugRenderToggle) if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera()) { //some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift) @@ -900,6 +945,10 @@ void PhysicsServerExample::renderScene() sprintf(bla,"VR sub-title text test,fps = %f, frame %d", 1./deltaTime, frameCount/2); float pos[4]; m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraTargetPosition(pos); + pos[0]+=gVRTeleportPos[0]; + pos[1]+=gVRTeleportPos[1]; + pos[2]+=gVRTeleportPos[2]; + btTransform viewTr; btScalar m[16]; float mf[16]; @@ -908,6 +957,9 @@ void PhysicsServerExample::renderScene() { m[i] = mf[i]; } + m[12]=+gVRTeleportPos[0]; + m[13]=+gVRTeleportPos[1]; + m[14]=+gVRTeleportPos[2]; viewTr.setFromOpenGLMatrix(m); btTransform viewTrInv = viewTr.inverse(); float upMag = -.6; @@ -952,7 +1004,7 @@ btVector3 PhysicsServerExample::getRayTo(int x,int y) btVector3 camPos,camTarget; renderer->getActiveCamera()->getCameraPosition(camPos); renderer->getActiveCamera()->getCameraTargetPosition(camTarget); - + btVector3 rayFrom = camPos; btVector3 rayForward = (camTarget-camPos); rayForward.normalize(); @@ -1025,17 +1077,54 @@ class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOpt } + + void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orn[4]) { - m_args[0].m_isPicking = (state!=0); - m_args[0].m_isReleasing = (state==0); - m_args[0].m_pos.setValue(pos[0],pos[1],pos[2]); - m_args[0].m_orn.setValue(orn[0],orn[1],orn[2],orn[3]); + //printf("controllerId %d, button=%d\n",controllerId, button); + + if (controllerId<0 || controllerId>MAX_VR_CONTROLLERS) + return; + + if (button==1 && state==0) + { + gVRTeleportPos = gLastPickPos; + } + if (button==32 && state==0) + { + gDebugRenderToggle = !gDebugRenderToggle; + } + + if (button==1) + { + m_args[0].m_isVrControllerTeleporting[controllerId] = true; + } + + if ((button==33)) + { + m_args[0].m_isVrControllerPicking[controllerId] = (state!=0); + m_args[0].m_isVrControllerReleasing[controllerId] = (state==0); + } + if ((button==33)||(button==1)) + { + m_args[0].m_vrControllerPos[controllerId].setValue(pos[0]+gVRTeleportPos[0],pos[1]+gVRTeleportPos[1],pos[2]+gVRTeleportPos[2]); + m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0],orn[1],orn[2],orn[3]); + } } +extern btVector3 gVRGripperPos; +extern btQuaternion gVRGripperOrn; + void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4]) { - m_args[0].m_pos.setValue(pos[0],pos[1],pos[2]); - m_args[0].m_orn.setValue(orn[0],orn[1],orn[2],orn[3]); + + m_args[0].m_vrControllerPos[controllerId].setValue(pos[0]+gVRTeleportPos[0],pos[1]+gVRTeleportPos[1],pos[2]+gVRTeleportPos[2]); + m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0],orn[1],orn[2],orn[3]); + + gVRGripperPos.setValue(pos[0]+gVRTeleportPos[0],pos[1]+gVRTeleportPos[1],pos[2]+gVRTeleportPos[2]); + btQuaternion orgOrn(orn[0],orn[1],orn[2],orn[3]); + gVRGripperOrn = orgOrn*btQuaternion(btVector3(0,0,1),SIMD_HALF_PI)*btQuaternion(btVector3(0,1,0),SIMD_HALF_PI); + + } B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc) diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index b718d40c7..2039cd40a 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -223,6 +223,7 @@ enum EnumSimParamUpdateFlags SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS=4, SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS=8, SIM_PARAM_UPDATE_REAL_TIME_SIMULATION = 16, + SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP=32 }; ///Controlling a robot involves sending the desired state to its joint motor controllers. @@ -234,6 +235,7 @@ struct SendPhysicsSimulationParameters int m_numSimulationSubSteps; int m_numSolverIterations; bool m_allowRealTimeSimulation; + double m_defaultContactERP; }; struct RequestActualStateArgs @@ -372,6 +374,22 @@ struct CalculateInverseDynamicsResultArgs double m_jointForces[MAX_DEGREE_OF_FREEDOM]; }; +struct CalculateInverseKinematicsArgs +{ + int m_bodyUniqueId; + double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM]; + double m_targetPosition[3]; + double m_targetOrientation[4];//orientation represented as quaternion, x,y,z,w +}; + +struct CalculateInverseKinematicsResultArgs +{ + int m_bodyUniqueId; + int m_dofCount; + double m_jointPositions[MAX_DEGREE_OF_FREEDOM]; +}; + + struct CreateJointArgs { int m_parentBodyIndex; @@ -413,6 +431,7 @@ struct SharedMemoryCommand struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments; struct CreateJointArgs m_createJointArguments; struct RequestContactDataArgs m_requestContactPointArguments; + struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments; }; }; @@ -445,6 +464,7 @@ struct SharedMemoryStatus struct RigidBodyCreateArgs m_rigidBodyCreateArgs; struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs; struct SendContactDataArgs m_sendContactPointArgs; + struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 28038a3ad..de76b2a98 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -69,6 +69,8 @@ enum EnumSharedMemoryServerStatus CMD_CALCULATED_INVERSE_DYNAMICS_FAILED, CMD_CONTACT_POINT_INFORMATION_COMPLETED, CMD_CONTACT_POINT_INFORMATION_FAILED, + CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED, + CMD_CALCULATE_INVERSE_KINEMATICS_FAILED, CMD_MAX_SERVER_COMMANDS }; diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index ba761dd82..0dd45af08 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -10,13 +10,15 @@ end includedirs {".","../../src", "../ThirdPartyLibs",} links { - "Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath" + "Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "BussIK" } language "C++" myfiles = { + "IKTrajectoryHelper.cpp", + "IKTrajectoryHelper.h", "PhysicsClient.cpp", "PhysicsClientSharedMemory.cpp", "PhysicsClientExample.cpp", @@ -134,10 +136,10 @@ else end defines {"B3_USE_STANDALONE_EXAMPLE"} -includedirs {"../../src"} +includedirs {"../../src", "../ThirdPartyLibs"} links { - "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common" + "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common","BussIK" } initOpenGL() initGlew() @@ -211,7 +213,7 @@ if os.is("Windows") then } links { - "BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath","OpenGL_Window","openvr_api" + "BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath","OpenGL_Window","openvr_api","BussIK" } diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index e85b8af0e..a9183b43c 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -13,7 +13,9 @@ #include "BulletCollision/CollisionShapes/btCollisionShape.h" #include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" +#include "LinearMath/btIDebugDraw.h" int gSharedMemoryKey = -1; +int gDebugDrawFlags = 0; //how can you try typing on a keyboard, without seeing it? //it is pretty funny, to see the desktop in VR! @@ -32,7 +34,7 @@ int gSharedMemoryKey = -1; CommonExampleInterface* sExample; int sPrevPacketNum=0; -GUIHelperInterface* sGuiPtr = 0; +OpenGLGuiHelper* sGuiPtr = 0; static vr::VRControllerState_t sPrevStates[vr::k_unMaxTrackedDeviceCount] = { 0 }; @@ -384,6 +386,8 @@ bool CMainApplication::BInit() sGuiPtr = new OpenGLGuiHelper(m_app,false); + sGuiPtr->setVRMode(true); + //sGuiPtr = new DummyGUIHelper; @@ -467,7 +471,7 @@ bool CMainApplication::BInit() m_fScaleSpacing = 4.0f; m_fNearClip = 0.1f; - m_fFarClip = 30.0f; + m_fFarClip = 3000.0f; m_iTexture = 0; m_uiVertcount = 0; @@ -668,6 +672,7 @@ bool CMainApplication::HandleInput() { //we need to have the 'move' events, so no early out here //if (sPrevStates[unDevice].unPacketNum != state.unPacketNum) + if( m_pHMD->GetTrackedDeviceClass( unDevice) == vr::TrackedDeviceClass_Controller ) { sPrevStates[unDevice].unPacketNum = state.unPacketNum; @@ -689,6 +694,14 @@ bool CMainApplication::HandleInput() if ((sPrevStates[unDevice].ulButtonPressed&trigger)==0) { // printf("Device PRESSED: %d, button %d\n", unDevice, button); + if (button==2) + { + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + gDebugDrawFlags = btIDebugDraw::DBG_DrawContactPoints + //btIDebugDraw::DBG_DrawConstraintLimits+ + //btIDebugDraw::DBG_DrawConstraints + ; + } sExample->vrControllerButtonCallback(unDevice, button, 1, pos, orn); } else @@ -699,16 +712,30 @@ bool CMainApplication::HandleInput() } else { - //not pressed now, but pressed before -> raise a button up event - if ((sPrevStates[unDevice].ulButtonPressed&trigger) != 0) + if( m_pHMD->GetTrackedDeviceClass( unDevice) == vr::TrackedDeviceClass_Controller ) { b3Transform tr; getControllerTransform(unDevice, tr); float pos[3] = { tr.getOrigin()[0], tr.getOrigin()[1], tr.getOrigin()[2] }; b3Quaternion born = tr.getRotation(); float orn[4] = { born[0], born[1], born[2], born[3] }; -// printf("Device RELEASED: %d, button %d\n", unDevice,button); - sExample->vrControllerButtonCallback(unDevice, button, 0, pos, orn); + // printf("Device RELEASED: %d, button %d\n", unDevice,button); + + //not pressed now, but pressed before -> raise a button up event + if ((sPrevStates[unDevice].ulButtonPressed&trigger) != 0) + { + if (button==2) + { + gDebugDrawFlags = 0; + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL); + } + + sExample->vrControllerButtonCallback(unDevice, button, 0, pos, orn); + } else + { + + sExample->vrControllerMoveCallback(unDevice, pos, orn); + } } } } @@ -1603,11 +1630,19 @@ void CMainApplication::RenderStereoTargets() m_app->m_instancingRenderer->setRenderFrameBuffer((unsigned int)leftEyeDesc.m_nRenderFramebufferId); - sExample->renderScene(); + if (gDebugDrawFlags) + { + sExample->physicsDebugDraw(gDebugDrawFlags); + } + + { + sExample->renderScene(); + } + //m_app->m_instancingRenderer->renderScene(); DrawGridData gridUp; gridUp.upAxis = m_app->getUpAxis(); - m_app->drawGrid(gridUp); +// m_app->drawGrid(gridUp); glBindFramebuffer( GL_FRAMEBUFFER, 0 ); @@ -1645,8 +1680,17 @@ void CMainApplication::RenderStereoTargets() m_app->m_instancingRenderer->setRenderFrameBuffer((unsigned int)rightEyeDesc.m_nRenderFramebufferId); //m_app->m_renderer->renderScene(); - sExample->renderScene(); - m_app->drawGrid(gridUp); + + if (gDebugDrawFlags) + { + sExample->physicsDebugDraw(gDebugDrawFlags); + } + + { + sExample->renderScene(); + } + + //m_app->drawGrid(gridUp); glBindFramebuffer( GL_FRAMEBUFFER, 0 ); diff --git a/examples/ThirdPartyLibs/BussIK/CMakeLists.txt b/examples/ThirdPartyLibs/BussIK/CMakeLists.txt new file mode 100644 index 000000000..96dc16025 --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/CMakeLists.txt @@ -0,0 +1,10 @@ + +INCLUDE_DIRECTORIES( + . +) + + +FILE(GLOB BussIK_SRCS "*.cpp" ) +FILE(GLOB BussIK_HDRS "*.h" ) + +ADD_LIBRARY(BussIK ${BussIK_SRCS} ${BussIK_HDRS}) diff --git a/examples/ThirdPartyLibs/BussIK/Misc.cpp b/examples/ThirdPartyLibs/BussIK/Misc.cpp index 6aa696d55..330997909 100644 --- a/examples/ThirdPartyLibs/BussIK/Misc.cpp +++ b/examples/ThirdPartyLibs/BussIK/Misc.cpp @@ -23,7 +23,7 @@ subject to the following restrictions: #include #include "LinearR3.h" -#include "../OpenGLWindow/OpenGLInclude.h" + /**************************************************************** Axes diff --git a/examples/ThirdPartyLibs/BussIK/premake4.lua b/examples/ThirdPartyLibs/BussIK/premake4.lua new file mode 100644 index 000000000..74a0630a0 --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/premake4.lua @@ -0,0 +1,11 @@ + project "BussIK" + + kind "StaticLib" + + includedirs { + "." + } + files { + "*.cpp", + "*.h", + } diff --git a/examples/pybullet/CMakeLists.txt b/examples/pybullet/CMakeLists.txt index a5afa5d66..f49c97b25 100644 --- a/examples/pybullet/CMakeLists.txt +++ b/examples/pybullet/CMakeLists.txt @@ -8,6 +8,8 @@ INCLUDE_DIRECTORIES( SET(pybullet_SRCS pybullet.c + ../../examples/SharedMemory/IKTrajectoryHelper.cpp + ../../examples/SharedMemory/IKTrajectoryHelper.h ../../examples/ExampleBrowser/InProcessExampleBrowser.cpp ../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp ../../examples/SharedMemory/TinyRendererVisualShapeConverter.h @@ -76,6 +78,6 @@ ADD_LIBRARY(pybullet SHARED ${pybullet_SRCS}) SET_TARGET_PROPERTIES(pybullet PROPERTIES VERSION ${BULLET_VERSION}) SET_TARGET_PROPERTIES(pybullet PROPERTIES SOVERSION ${BULLET_VERSION}) -TARGET_LINK_LIBRARIES(pybullet BulletExampleBrowserLib BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen Bullet3Common ${PYTHON_LIBRARIES}) +TARGET_LINK_LIBRARIES(pybullet BulletExampleBrowserLib BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK Bullet3Common ${PYTHON_LIBRARIES}) diff --git a/examples/pybullet/premake4.lua b/examples/pybullet/premake4.lua index cacb7218e..86b4eedc5 100644 --- a/examples/pybullet/premake4.lua +++ b/examples/pybullet/premake4.lua @@ -10,7 +10,7 @@ project ("pybullet") defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"} hasCL = findOpenCL("clew") - links{"BulletExampleBrowserLib","gwen", "BulletFileLoader","BulletWorldImporter","OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"} + links{"BulletExampleBrowserLib","gwen", "BulletFileLoader","BulletWorldImporter","OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK", "Bullet3Common"} initOpenGL() initGlew() @@ -36,6 +36,8 @@ project ("pybullet") files { "pybullet.c", + "../../examples/SharedMemory/IKTrajectoryHelper.cpp", + "../../examples/SharedMemory/IKTrajectoryHelper.h", "../../examples/ExampleBrowser/InProcessExampleBrowser.cpp", "../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp", "../../examples/SharedMemory/TinyRendererVisualShapeConverter.h", diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 592d3d4e3..f9fda2967 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -593,6 +593,36 @@ pybullet_setTimeStep(PyObject* self, PyObject* args) return Py_None; } +static PyObject * +pybullet_setDefaultContactERP(PyObject* self, PyObject* args) +{ + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + double defaultContactERP=0.005; + int ret; + + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; + + if (!PyArg_ParseTuple(args, "d", &defaultContactERP)) + { + PyErr_SetString(SpamError, "default Contact ERP expected a single value (double)."); + return NULL; + } + ret = b3PhysicsParamSetDefaultContactERP(command, defaultContactERP); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + } + + Py_INCREF(Py_None); + return Py_None; +} + // Internal function used to get the base position and orientation @@ -1865,6 +1895,10 @@ static PyMethodDef SpamMethods[] = { {"setTimeStep", pybullet_setTimeStep, METH_VARARGS, "Set the amount of time to proceed at each call to stepSimulation. (unit is seconds, typically range is 0.01 or 0.001)"}, + + {"setDefaultContactERP", pybullet_setDefaultContactERP, METH_VARARGS, + "Set the amount of contact penetration Error Recovery Paramater (ERP) in each time step. \ + This is an tuning parameter to control resting contact stability. It depends on the time step. For 1/240 timestep, 0.005 is a reasonable values."}, { "setRealTimeSimulation", pybullet_setRealTimeSimulation, METH_VARARGS, "Enable or disable real time simulation (using the real time clock, RTC) in the physics server. Expects one integer argument, 0 or 1" }, diff --git a/src/Bullet3Common/b3Scalar.h b/src/Bullet3Common/b3Scalar.h index 60b7f1cfd..16a9ac7a7 100644 --- a/src/Bullet3Common/b3Scalar.h +++ b/src/Bullet3Common/b3Scalar.h @@ -55,7 +55,7 @@ inline int b3GetVersion() //#define B3_HAS_ALIGNED_ALLOCATOR #pragma warning(disable : 4324) // disable padding warning // #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning. -// #pragma warning(disable:4996) //Turn off warnings about deprecated C routines + #pragma warning(disable:4996) //Turn off warnings about deprecated C routines // #pragma warning(disable:4786) // Disable the "debug name too long" warning #define B3_FORCE_INLINE __forceinline diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp index 8d9aa77eb..847e5c6c7 100644 --- a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp +++ b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp @@ -609,6 +609,7 @@ static inline int jointNumDoFs(const JointType &type) { error_message("invalid joint type\n"); // TODO add configurable abort/crash function abort(); + return 0; } int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool update_kinematics, diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h index 011fa1392..df907e128 100644 --- a/src/LinearMath/btScalar.h +++ b/src/LinearMath/btScalar.h @@ -58,7 +58,7 @@ inline int btGetVersion() //#define BT_HAS_ALIGNED_ALLOCATOR #pragma warning(disable : 4324) // disable padding warning // #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning. -// #pragma warning(disable:4996) //Turn off warnings about deprecated C routines + #pragma warning(disable:4996) //Turn off warnings about deprecated C routines // #pragma warning(disable:4786) // Disable the "debug name too long" warning #define SIMD_FORCE_INLINE __forceinline diff --git a/test/SharedMemory/CMakeLists.txt b/test/SharedMemory/CMakeLists.txt index b7bc9a255..496ca5076 100644 --- a/test/SharedMemory/CMakeLists.txt +++ b/test/SharedMemory/CMakeLists.txt @@ -13,7 +13,7 @@ ADD_DEFINITIONS(-DPHYSICS_LOOP_BACK -DPHYSICS_SERVER_DIRECT -DENABLE_GTEST -D_VA LINK_LIBRARIES( - BulletInverseDynamicsUtils BulletInverseDynamics BulletFileLoader BulletWorldImporter Bullet3Common BulletDynamics BulletCollision LinearMath gtest + BulletInverseDynamicsUtils BulletInverseDynamics BulletFileLoader BulletWorldImporter Bullet3Common BulletDynamics BulletCollision LinearMath gtest BussIK ) IF (NOT WIN32) @@ -23,6 +23,8 @@ ENDIF() ADD_EXECUTABLE(Test_PhysicsClientServer gtestwrap.cpp ../../examples/SharedMemory/PhysicsClient.cpp + ../../examples/SharedMemory/IKTrajectoryHelper.cpp + ../../examples/SharedMemory/IKTrajectoryHelper.h ../../examples/SharedMemory/PhysicsClient.h ../../examples/SharedMemory/PhysicsServer.cpp ../../examples/SharedMemory/PhysicsServer.h diff --git a/test/SharedMemory/premake4.lua b/test/SharedMemory/premake4.lua index 466ad76a4..1974ba471 100644 --- a/test/SharedMemory/premake4.lua +++ b/test/SharedMemory/premake4.lua @@ -43,11 +43,14 @@ project ("Test_PhysicsServerLoopBack") "Bullet3Common", "BulletDynamics", "BulletCollision", + "BussIK", "LinearMath" } files { "test.c", + "../../examples/SharedMemory/IKTrajectoryHelper.cpp", + "../../examples/SharedMemory/IKTrajectoryHelper.h", "../../examples/SharedMemory/PhysicsClient.cpp", "../../examples/SharedMemory/PhysicsClient.h", "../../examples/SharedMemory/PhysicsServer.cpp", @@ -112,12 +115,15 @@ project ("Test_PhysicsServerLoopBack") "BulletWorldImporter", "Bullet3Common", "BulletDynamics", - "BulletCollision", + "BulletCollision", + "BussIK", "LinearMath" } files { "test.c", + "../../examples/SharedMemory/IKTrajectoryHelper.cpp", + "../../examples/SharedMemory/IKTrajectoryHelper.h", "../../examples/SharedMemory/PhysicsClient.cpp", "../../examples/SharedMemory/PhysicsClient.h", "../../examples/SharedMemory/PhysicsServer.cpp", @@ -187,7 +193,7 @@ project ("Test_PhysicsServerInProcessExampleBrowser") -- } hasCL = findOpenCL("clew") - links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletFileLoader","BulletWorldImporter","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"} + links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletFileLoader","BulletWorldImporter","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK","Bullet3Common"} initOpenGL() initGlew() @@ -214,6 +220,8 @@ project ("Test_PhysicsServerInProcessExampleBrowser") files { "test.c", + "../../examples/SharedMemory/IKTrajectoryHelper.cpp", + "../../examples/SharedMemory/IKTrajectoryHelper.h", "../../examples/ExampleBrowser/InProcessExampleBrowser.cpp", "../../examples/SharedMemory/InProcessMemory.cpp", "../../examples/SharedMemory/PhysicsClient.cpp", From 016dc273c6ebe5c29a69c711e9bd09393d77d898 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 9 Sep 2016 09:01:04 -0700 Subject: [PATCH 11/14] fix an SSE/SIMD 16-byte alignment issue --- examples/OpenGLWindow/SimpleCamera.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/examples/OpenGLWindow/SimpleCamera.cpp b/examples/OpenGLWindow/SimpleCamera.cpp index a78f4de25..f0ba375d2 100644 --- a/examples/OpenGLWindow/SimpleCamera.cpp +++ b/examples/OpenGLWindow/SimpleCamera.cpp @@ -6,7 +6,7 @@ #include "Bullet3Common/b3Transform.h" -struct SimpleCameraInternalData +B3_ATTRIBUTE_ALIGNED16(struct) SimpleCameraInternalData { SimpleCameraInternalData() :m_cameraTargetPosition(b3MakeVector3(0,0,0)), @@ -25,7 +25,11 @@ struct SimpleCameraInternalData tr.setIdentity(); tr.getOpenGLMatrix(m_offsetTransformVR); } - b3Vector3 m_cameraTargetPosition; + + B3_DECLARE_ALIGNED_ALLOCATOR(); + + B3_ATTRIBUTE_ALIGNED16(float) m_offsetTransformVR[16]; + b3Vector3 m_cameraTargetPosition; float m_cameraDistance; b3Vector3 m_cameraUp; b3Vector3 m_cameraForward; @@ -42,8 +46,7 @@ struct SimpleCameraInternalData bool m_enableVR; float m_viewMatrixVR[16]; float m_projectionMatrixVR[16]; - float m_offsetTransformVR[16]; - + }; From f72982306ea3c12ae181630bbf14a54670581262 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Fri, 9 Sep 2016 11:28:38 -0700 Subject: [PATCH 12/14] fix issues with VR physics server --- .../PhysicsServerCommandProcessor.cpp | 28 +++++--- .../SharedMemory/PhysicsServerExample.cpp | 70 +++++++++++++------ .../StandaloneMain/hellovr_opengl_main.cpp | 8 +-- 3 files changed, 69 insertions(+), 37 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index cb5a3b422..94546637e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -556,6 +556,9 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); + //Workaround: in a VR application, where we avoid synchronizaing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it + m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(8192); + m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer(); @@ -2694,13 +2697,16 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName) btVector3 gVRGripperPos(0,0,0); btQuaternion gVRGripperOrn(0,0,0,1); - +int gDroppedSimulationSteps = 0; +int gNumSteps = 0; +double gDtInSec = 0.f; +double gSubStep = 0.f; void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) { if (m_data->m_allowRealTimeSimulation && m_data->m_guiHelper) { - btAlignedObjectArray bufferServerToClient; - bufferServerToClient.resize(32768); + static btAlignedObjectArray gBufferServerToClient; + gBufferServerToClient.resize(32768); if (!m_data->m_hasGround) @@ -2710,14 +2716,14 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) int bodyId = 0; - loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &bufferServerToClient[0], bufferServerToClient.size()); + loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); } if (0)//m_data->m_gripperRigidbodyFixed==0) { int bodyId = 0; - loadUrdf("pr2_gripper.urdf",btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &bufferServerToClient[0], bufferServerToClient.size()); + loadUrdf("pr2_gripper.urdf",btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); InteralBodyData* parentBody = m_data->getHandle(bodyId); if (parentBody->m_multiBody) @@ -2747,13 +2753,13 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) int maxSteps = 3; int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec,maxSteps,m_data->m_physicsDeltaTime); - int droppedSteps = numSteps > maxSteps ? numSteps - maxSteps : 0; - static int skipReport = 0; - skipReport++; - if (skipReport>100) + gDroppedSimulationSteps += numSteps > maxSteps ? numSteps - maxSteps : 0; + + if (numSteps) { - skipReport = 0; - //printf("numSteps: %d (dropped %d), %f (internal step = %f)\n", numSteps, droppedSteps , dtInSec, m_data->m_physicsDeltaTime); + gNumSteps = numSteps; + gDtInSec = dtInSec; + gSubStep = m_data->m_physicsDeltaTime; } } } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index ad5fa0fd3..49f63a9a4 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -13,7 +13,8 @@ #include "../Utils/b3Clock.h" #include "../MultiThreading/b3ThreadSupportInterface.h" -#define MAX_VR_CONTROLLERS 4 +#define MAX_VR_CONTROLLERS 8 + extern btVector3 gLastPickPos; btVector3 gVRTeleportPos(0,0,0); @@ -880,6 +881,12 @@ static float vrOffset[16]={1,0,0,0, 0,0,0,0}; +extern int gDroppedSimulationSteps; +extern int gNumSteps; +extern double gDtInSec; +extern double gSubStep; + + void PhysicsServerExample::renderScene() { ///debug rendering @@ -925,24 +932,38 @@ void PhysicsServerExample::renderScene() //some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift) static int frameCount=0; - frameCount++; - char bla[1024]; - static btScalar prevTime = m_clock.getTimeSeconds(); - btScalar curTime = m_clock.getTimeSeconds(); - static btScalar deltaTime = 0.f; - static int count = 10; - if (count-- < 0) - { - count = 10; - deltaTime = curTime - prevTime; - } - if (deltaTime == 0) - deltaTime = 1000; + frameCount++; + static char line0[1024]; + static char line1[1024]; + + static btScalar worseFps = 1000000; + int numFrames = 200; + static int count = 0; + count++; + + if (0 == (count & 1)) + { + btScalar curTime = m_clock.getTimeSeconds(); + btScalar fps = 1. / (curTime - prevTime); + prevTime = curTime; + if (fps < worseFps) + { + worseFps = fps; + } + + if (count > numFrames) + { + count = 0; + sprintf(line0, "Graphics FPS (worse) = %f, frame %d", worseFps, frameCount / 2); + + sprintf(line1, "Physics Steps = %d, Dropped = %d, dt %f, Substep %f)", gNumSteps, gDroppedSimulationSteps, gDtInSec, gSubStep); + gDroppedSimulationSteps = 0; + + worseFps = 1000000; + } + } - prevTime = curTime; - - sprintf(bla,"VR sub-title text test,fps = %f, frame %d", 1./deltaTime, frameCount/2); float pos[4]; m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraTargetPosition(pos); pos[0]+=gVRTeleportPos[0]; @@ -965,12 +986,12 @@ void PhysicsServerExample::renderScene() float upMag = -.6; btVector3 side = viewTrInv.getBasis().getColumn(0); btVector3 up = viewTrInv.getBasis().getColumn(1); - up+=0.35*side; - m_guiHelper->getAppInterface()->drawText3D(bla,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1); + up+=0.6*side; + m_guiHelper->getAppInterface()->drawText3D(line0,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1); //btVector3 fwd = viewTrInv.getBasis().getColumn(2); - sprintf(bla,"VR line 2 sub-title text test, frame %d", frameCount/2); + upMag = -0.7; - m_guiHelper->getAppInterface()->drawText3D(bla,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1); + m_guiHelper->getAppInterface()->drawText3D(line1,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1); } //m_args[0].m_cs->unlock(); @@ -1083,7 +1104,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt { //printf("controllerId %d, button=%d\n",controllerId, button); - if (controllerId<0 || controllerId>MAX_VR_CONTROLLERS) + if (controllerId<0 || controllerId>=MAX_VR_CONTROLLERS) return; if (button==1 && state==0) @@ -1118,6 +1139,11 @@ extern btQuaternion gVRGripperOrn; void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4]) { + if (controllerId <= 0 || controllerId >= MAX_VR_CONTROLLERS) + { + printf("Controller Id exceeds max: %d > %d", controllerId, MAX_VR_CONTROLLERS); + return; + } m_args[0].m_vrControllerPos[controllerId].setValue(pos[0]+gVRTeleportPos[0],pos[1]+gVRTeleportPos[1],pos[2]+gVRTeleportPos[2]); m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0],orn[1],orn[2],orn[3]); diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index a9183b43c..aaa453fce 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -625,6 +625,9 @@ void CMainApplication::Shutdown() } } + sExample->exitPhysics(); + delete sExample; + delete m_app; m_app=0; @@ -752,13 +755,11 @@ bool CMainApplication::HandleInput() //----------------------------------------------------------------------------- // Purpose: //----------------------------------------------------------------------------- + void CMainApplication::RunMainLoop() { bool bQuit = false; - //SDL_StartTextInput(); - //SDL_ShowCursor( SDL_DISABLE ); - while ( !bQuit && !m_app->m_window->requestedExit()) { bQuit = HandleInput(); @@ -766,7 +767,6 @@ void CMainApplication::RunMainLoop() RenderFrame(); } - //SDL_StopTextInput(); } From 3c706306cdd2e9fd22d5d04825207cb32580a2b7 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Fri, 9 Sep 2016 14:30:37 -0700 Subject: [PATCH 13/14] add experimental pr2-gripper support in VR physics server add setErp to btMultiBodyJointMotor --- build_visual_studio.bat | 4 +- data/pr2_gripper.urdf | 126 ++++++++++++++++++ .../PhysicsServerCommandProcessor.cpp | 84 ++++++++---- .../SharedMemory/PhysicsServerExample.cpp | 41 ++++-- .../StandaloneMain/hellovr_opengl_main.cpp | 6 +- .../Featherstone/btMultiBodyJointMotor.cpp | 5 +- .../Featherstone/btMultiBodyJointMotor.h | 9 ++ 7 files changed, 234 insertions(+), 41 deletions(-) create mode 100644 data/pr2_gripper.urdf diff --git a/build_visual_studio.bat b/build_visual_studio.bat index 08e6b70b6..9cefeb7d2 100644 --- a/build_visual_studio.bat +++ b/build_visual_studio.bat @@ -2,9 +2,9 @@ rem premake4 --with-pe vs2010 rem premake4 --bullet2demos vs2010 cd build3 -premake4 --enable_openvr --targetdir="../bin" vs2010 +rem premake4 --enable_openvr --targetdir="../bin" vs2010 -rem premake4 --enable_pybullet --python_include_dir="c:/python-3.5.2/include" --python_lib_dir="c:/python-3.5.2/libs" --enable_openvr --targetdir="../bin" vs2010 +premake4 --enable_pybullet --python_include_dir="c:/python34/include" --python_lib_dir="c:/python34/libs" --enable_openvr --targetdir="../bin" vs2010 rem premake4 --enable_openvr --enable_pybullet --targetdir="../bin" vs2010 rem premake4 --targetdir="../server2bin" vs2010 rem cd vs2010 diff --git a/data/pr2_gripper.urdf b/data/pr2_gripper.urdf new file mode 100644 index 000000000..33d833629 --- /dev/null +++ b/data/pr2_gripper.urdf @@ -0,0 +1,126 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 94546637e..d2a83cc56 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -382,6 +382,7 @@ struct PhysicsServerCommandProcessorInternalData bool m_hasGround; btMultiBodyFixedConstraint* m_gripperRigidbodyFixed; + btMultiBody* m_gripperMultiBody; CommandLogger* m_commandLogger; CommandLogPlayback* m_logPlayback; @@ -432,6 +433,7 @@ struct PhysicsServerCommandProcessorInternalData PhysicsServerCommandProcessorInternalData() :m_hasGround(false), m_gripperRigidbodyFixed(0), + m_gripperMultiBody(0), m_allowRealTimeSimulation(true), m_commandLogger(0), m_logPlayback(0), @@ -2697,6 +2699,9 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName) btVector3 gVRGripperPos(0,0,0); btQuaternion gVRGripperOrn(0,0,0,1); +bool gVRGripperClosed = false; + + int gDroppedSimulationSteps = 0; int gNumSteps = 0; double gDtInSec = 0.f; @@ -2717,37 +2722,70 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - } - - if (0)//m_data->m_gripperRigidbodyFixed==0) - { - int bodyId = 0; - - loadUrdf("pr2_gripper.urdf",btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - - InteralBodyData* parentBody = m_data->getHandle(bodyId); - if (parentBody->m_multiBody) + + if (m_data->m_gripperRigidbodyFixed == 0) { - parentBody->m_multiBody->setHasSelfCollision(1); - btVector3 pivotInParent(0,0,0); - btMatrix3x3 frameInParent; - frameInParent.setRotation(btQuaternion(0,0,0,1)); - - btVector3 pivotInChild(0,0,0); - btMatrix3x3 frameInChild; - frameInChild.setIdentity(); + int bodyId = 0; - m_data->m_gripperRigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,-1,0,pivotInParent,pivotInChild,frameInParent,frameInChild); - m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(2.); - btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; - world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed); + if (loadUrdf("pr2_gripper.urdf", btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size())) + { + InteralBodyData* parentBody = m_data->getHandle(bodyId); + if (parentBody->m_multiBody) + { + parentBody->m_multiBody->setHasSelfCollision(1); + btVector3 pivotInParent(0, 0, 0); + btMatrix3x3 frameInParent; + frameInParent.setRotation(btQuaternion(0, 0, 0, 1)); + + btVector3 pivotInChild(0, 0, 0); + btMatrix3x3 frameInChild; + frameInChild.setIdentity(); + + m_data->m_gripperRigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody, -1, 0, pivotInParent, pivotInChild, frameInParent, frameInChild); + m_data->m_gripperMultiBody = parentBody->m_multiBody; + if (m_data->m_gripperMultiBody->getNumLinks() > 2) + { + m_data->m_gripperMultiBody->setJointPos(0, SIMD_HALF_PI); + m_data->m_gripperMultiBody->setJointPos(2, SIMD_HALF_PI); + } + m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(2.); + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld; + world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed); + } + } } } - if (m_data->m_gripperRigidbodyFixed) + if (m_data->m_gripperRigidbodyFixed && m_data->m_gripperMultiBody) { m_data->m_gripperRigidbodyFixed->setFrameInB(btMatrix3x3(gVRGripperOrn)); m_data->m_gripperRigidbodyFixed->setPivotInB(gVRGripperPos); + if (m_data->m_gripperMultiBody->getNumLinks() > 2) + { + for (int i = 0; i < 2; i++) + { + if (supportsJointMotor(m_data->m_gripperMultiBody, i * 2)) + { + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i * 2).m_userPtr; + if (motor) + { + motor->setErp(0.005); + + if (gVRGripperClosed) + { + motor->setPositionTarget(0.2, 1); + } + else + { + motor->setPositionTarget(SIMD_HALF_PI, 1); + } + motor->setVelocityTarget(0, 0.1); + btScalar maxImp = 550.*m_data->m_physicsDeltaTime; + motor->setMaxAppliedImpulse(maxImp); + } + } + } + } } int maxSteps = 3; diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 49f63a9a4..f84e7356c 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -18,6 +18,8 @@ extern btVector3 gLastPickPos; btVector3 gVRTeleportPos(0,0,0); +extern bool gVRGripperClosed; + bool gDebugRenderToggle = false; void MotionThreadFunc(void* userPtr,void* lsMemory); void* MotionlsMemoryFunc(); @@ -1116,20 +1118,29 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt gDebugRenderToggle = !gDebugRenderToggle; } + if (button==1) { m_args[0].m_isVrControllerTeleporting[controllerId] = true; } - if ((button==33)) + if (controllerId == 3 && (button == 33)) { - m_args[0].m_isVrControllerPicking[controllerId] = (state!=0); - m_args[0].m_isVrControllerReleasing[controllerId] = (state==0); + gVRGripperClosed =state; } - if ((button==33)||(button==1)) + else { - m_args[0].m_vrControllerPos[controllerId].setValue(pos[0]+gVRTeleportPos[0],pos[1]+gVRTeleportPos[1],pos[2]+gVRTeleportPos[2]); - m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0],orn[1],orn[2],orn[3]); + + if ((button == 33)) + { + m_args[0].m_isVrControllerPicking[controllerId] = (state != 0); + m_args[0].m_isVrControllerReleasing[controllerId] = (state == 0); + } + if ((button == 33) || (button == 1)) + { + m_args[0].m_vrControllerPos[controllerId].setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]); + m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0], orn[1], orn[2], orn[3]); + } } } @@ -1144,13 +1155,17 @@ void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[ printf("Controller Id exceeds max: %d > %d", controllerId, MAX_VR_CONTROLLERS); return; } - m_args[0].m_vrControllerPos[controllerId].setValue(pos[0]+gVRTeleportPos[0],pos[1]+gVRTeleportPos[1],pos[2]+gVRTeleportPos[2]); - m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0],orn[1],orn[2],orn[3]); - - gVRGripperPos.setValue(pos[0]+gVRTeleportPos[0],pos[1]+gVRTeleportPos[1],pos[2]+gVRTeleportPos[2]); - btQuaternion orgOrn(orn[0],orn[1],orn[2],orn[3]); - gVRGripperOrn = orgOrn*btQuaternion(btVector3(0,0,1),SIMD_HALF_PI)*btQuaternion(btVector3(0,1,0),SIMD_HALF_PI); - + if (controllerId == 3) + { + gVRGripperPos.setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]); + btQuaternion orgOrn(orn[0], orn[1], orn[2], orn[3]); + gVRGripperOrn = orgOrn*btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI); + } + else + { + m_args[0].m_vrControllerPos[controllerId].setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]); + m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0], orn[1], orn[2], orn[3]); + } } B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc) diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index aaa453fce..d806ad27c 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -700,12 +700,16 @@ bool CMainApplication::HandleInput() if (button==2) { glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); - gDebugDrawFlags = btIDebugDraw::DBG_DrawContactPoints + ///todo(erwincoumans) can't use reguar debug drawer, because physics/graphics are not in sync + //add a special debug drawer that deals with this + //gDebugDrawFlags = btIDebugDraw::DBG_DrawContactPoints //btIDebugDraw::DBG_DrawConstraintLimits+ //btIDebugDraw::DBG_DrawConstraints ; } + sExample->vrControllerButtonCallback(unDevice, button, 1, pos, orn); + } else { diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index 326a6ac48..3762e17a5 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -26,7 +26,8 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScal m_desiredVelocity(desiredVelocity), m_desiredPosition(0), m_kd(1.), - m_kp(0) + m_kp(0), + m_erp(1) { m_maxAppliedImpulse = maxMotorImpulse; @@ -123,7 +124,7 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con int dof = 0; btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof]; btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; - btScalar positionStabiliationTerm = (m_desiredPosition-currentPosition)/infoGlobal.m_timeStep; + btScalar positionStabiliationTerm = m_erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep; btScalar velocityError = (m_desiredVelocity - currentVelocity); btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h index a8bcf606c..ceac3b65e 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h @@ -29,6 +29,7 @@ protected: btScalar m_desiredPosition; btScalar m_kd; btScalar m_kp; + btScalar m_erp; public: @@ -57,6 +58,14 @@ public: m_kp = kp; } + virtual void setErp(btScalar erp) + { + m_erp = erp; + } + virtual btScalar getErp() const + { + return m_erp; + } virtual void debugDraw(class btIDebugDraw* drawer) { From c5d775a63504e944dde7d7e469c6e4b740126df6 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Fri, 9 Sep 2016 14:41:23 -0700 Subject: [PATCH 14/14] disable real-time by default --- 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 d2a83cc56..8f6f19e0b 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -434,7 +434,7 @@ struct PhysicsServerCommandProcessorInternalData :m_hasGround(false), m_gripperRigidbodyFixed(0), m_gripperMultiBody(0), - m_allowRealTimeSimulation(true), + m_allowRealTimeSimulation(false), m_commandLogger(0), m_logPlayback(0), m_physicsDeltaTime(1./240.),