diff --git a/Extras/InverseDynamics/btMultiBodyFromURDF.hpp b/Extras/InverseDynamics/btMultiBodyFromURDF.hpp index 6b46649ec..63455c399 100644 --- a/Extras/InverseDynamics/btMultiBodyFromURDF.hpp +++ b/Extras/InverseDynamics/btMultiBodyFromURDF.hpp @@ -30,6 +30,7 @@ public: m_collisionConfiguration = 0x0; m_dynamicsWorld = 0x0; m_multibody = 0x0; + m_flag = 0x0; } /// dtor ~MyBtMultiBodyFromURDF() @@ -43,6 +44,7 @@ public: } /// @param name path to urdf file void setFileName(const std::string name) { m_filename = name; } + void setFlag(int flag) { m_flag = flag; } /// load urdf file and build btMultiBody model void init() { @@ -60,7 +62,7 @@ public: MyMultiBodyCreator creation(&m_nogfx); const bool use_multibody = true; ConvertURDF2Bullet(u2b, creation, identityTrans, m_dynamicsWorld, use_multibody, - u2b.getPathPrefix()); + u2b.getPathPrefix(), m_flag); m_multibody = creation.getBulletMultiBody(); m_dynamicsWorld->stepSimulation(1. / 240., 0); } @@ -94,5 +96,6 @@ private: btMultiBody *m_multibody; const btVector3 m_gravity; const bool m_base_fixed; + int m_flag; }; #endif // BTMULTIBODYFROMURDF_HPP diff --git a/data/pantilt.urdf b/data/pantilt.urdf new file mode 100755 index 000000000..45879d811 --- /dev/null +++ b/data/pantilt.urdf @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/test/InverseDynamics/CMakeLists.txt b/test/InverseDynamics/CMakeLists.txt index 290b91dc1..292d4c477 100644 --- a/test/InverseDynamics/CMakeLists.txt +++ b/test/InverseDynamics/CMakeLists.txt @@ -1,5 +1,70 @@ +IF (BULLET_BUILD_RBDL_COMPARE_TEST) + INCLUDE_DIRECTORIES( + . + ../../src + ../gtest-1.7.0/include + ../../examples/ThirdPartyLibs + ${BULLET_RBDL_SOURCE_DIR}/include/ + ${BULLET_RBDL_SOURCE_DIR}/addons/urdfreader/ + ${BULLET_RBDL_BINARY_DIR}/include + ) + + ADD_DEFINITIONS(-D_VARIADIC_MAX=10) + ADD_DEFINITIONS(-DRBDL_USE_SIMPLE_MATH) + IF (MSVC) + LINK_LIBRARIES( BulletDynamics BulletCollision Bullet3Common LinearMath gtest rbdl_urdfreader-static rbdl-static) + ELSE (MSVC) + LINK_LIBRARIES( BulletDynamics BulletCollision Bullet3Common LinearMath gtest rbdl_urdfreader rbdl) + ENDIF (MSVC) + IF (NOT WIN32) + LINK_LIBRARIES( pthread ) + ENDIF() + + + ADD_EXECUTABLE(Test_DynamicsABA_bulletVSrbdl + test_dynABA_bulletVSrbdl.cpp + ../../examples/Utils/b3ResourcePath.cpp + ../../examples/Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h + ../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp + ../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h + ../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp + ../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.h + ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp + ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp + ../../examples/Importers/ImportURDFDemo/UrdfParser.h + ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp + ../../examples/Importers/ImportURDFDemo/URDF2Bullet.h + ../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp + ../../examples/ThirdPartyLibs/stb_image/stb_image.cpp + ../../examples/Utils/b3Clock.cpp + ../../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp + ../../Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp + ../../Extras/Serialize/BulletFileLoader/bChunk.cpp + ../../Extras/Serialize/BulletFileLoader/bFile.cpp + ../../Extras/Serialize/BulletFileLoader/bDNA.cpp + ../../Extras/Serialize/BulletFileLoader/btBulletFile.cpp + ../../examples/Importers/ImportURDFDemo/URDFImporterInterface.h + ../../examples/Importers/ImportURDFDemo/URDFJointTypes.h + ../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp + ../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp + ../../examples/Importers/ImportSTLDemo/ImportSTLSetup.h + ../../examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h + ../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp + ../../examples/Importers/ImportColladaDemo/ColladaGraphicsInstance.h + ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp + ../../examples/ThirdPartyLibs/tinyxml2/tinyxml2.cpp + + ) + + ADD_TEST(Test_DynamicsABA_bulletVSrbdl_PASS Test_DynamicsABA_bulletVSrbdl) + + IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) + SET_TARGET_PROPERTIES(Test_DynamicsABA_bulletVSrbdl PROPERTIES DEBUG_POSTFIX "_Debug") + SET_TARGET_PROPERTIES(Test_DynamicsABA_bulletVSrbdl PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel") + SET_TARGET_PROPERTIES(Test_DynamicsABA_bulletVSrbdl PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo") + ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) - +ENDIF (BULLET_BUILD_RBDL_COMPARE_TEST) INCLUDE_DIRECTORIES( . diff --git a/test/InverseDynamics/test_dynABA_bulletVSrbdl.cpp b/test/InverseDynamics/test_dynABA_bulletVSrbdl.cpp new file mode 100644 index 000000000..40b14d99a --- /dev/null +++ b/test/InverseDynamics/test_dynABA_bulletVSrbdl.cpp @@ -0,0 +1,376 @@ +/// Then - run forward dynamics on random input data (q, u, dot_u) to get forces with rbdl +/// - run forward dynamics on random input data (q, u, dot_u) to get forces with bullet +/// - compare input accelerations + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include "../../examples/CommonInterfaces/CommonGUIHelperInterface.h" +#include "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.h" +#include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h" +#include "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h" +#include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h" +#include "../../examples/Utils/b3ResourcePath.h" +#include "../../examples/SharedMemory/SharedMemoryPublic.h" +#include +#include +#include "Bullet3Common/b3CommandLineArgs.h" +#include "Bullet3Common/b3Random.h" + +#include +#include +#include "urdfreader.h" + +using namespace std; +using namespace btInverseDynamics; +using namespace RigidBodyDynamics::Math; + +bool verbose = false; +bool floatbase = false; + +bool FLAGS_verbose = false; + +static btVector3 gravity(0., 0., -10.); +static const bool kBaseFixed = true; +static const char kUrdfFile[] = "pantilt.urdf"; + + +int compareABAForwardDynamicsBulletAndRbdl(btAlignedObjectArray &q, btAlignedObjectArray &u, btAlignedObjectArray &joint_forces, btAlignedObjectArray &dot_u, btVector3 &gravity, bool verbose, + btMultiBody *btmb, double *acc_error) +{ +// call function and return -1 if it does, printing an bt_id_error_message +#define RETURN_ON_FAILURE(x) \ + do \ + { \ + if (-1 == x) \ + { \ + bt_id_error_message("calling " #x "\n"); \ + return -1; \ + } \ + } while (0) + + if (verbose) + { + printf("\n ===================================== \n"); + } + + // set positions and velocities for btMultiBody + // base link + btMatrix3x3 world_T_base; + btVector3 world_pos_base; + btVector3 base_velocity; + btVector3 base_angular_velocity; + + world_T_base.setIdentity(); + base_velocity.setZero(); + base_angular_velocity.setZero(); + + btmb->setBaseOmega(base_angular_velocity); + btmb->setBaseVel(base_velocity); + btmb->setLinearDamping(0); + btmb->setAngularDamping(0); + + // remaining links + int q_index; + if (btmb->hasFixedBase()) + { + q_index = 0; + } + else + { + q_index = 6; + } + if (verbose) + { + printf("bt:num_links= %d, num_dofs= %d\n", btmb->getNumLinks(), btmb->getNumDofs()); + } + for (int l = 0; l < btmb->getNumLinks(); l++) + { + const btMultibodyLink &link = btmb->getLink(l); + if (verbose) + { + printf("link %d, pos_var_count= %d, dof_count= %d\n", l, link.m_posVarCount, + link.m_dofCount); + } + if (link.m_posVarCount == 1) + { + btmb->setJointPosMultiDof(l, &q[q_index]); + btmb->setJointVelMultiDof(l, &u[q_index]); + if (verbose) + { + printf("set q[%d]= %f, u[%d]= %f\n", q_index, q[q_index], q_index, u[q_index]); + } + q_index++; + } + } + // sanity check + if (q_index != q.size()) + { + bt_id_error_message("error in number of dofs for btMultibody and MultiBodyTree\n"); + return -1; + } + + // set up bullet forward dynamics model + btScalar dt = 0; + btAlignedObjectArray scratch_r; + btAlignedObjectArray scratch_v; + btAlignedObjectArray scratch_m; + // this triggers switch between using either appliedConstraintForce or appliedForce + bool isConstraintPass = false; + // apply gravity forces for btMultiBody model. Must be done manually. + btmb->addBaseForce(btmb->getBaseMass() * gravity); + + for (int link = 0; link < btmb->getNumLinks(); link++) + { + btmb->addLinkForce(link, gravity * btmb->getLinkMass(link)); + if (verbose) + { + printf("link %d, applying gravity %f %f %f\n", link, + gravity[0] * btmb->getLinkMass(link), gravity[1] * btmb->getLinkMass(link), + gravity[2] * btmb->getLinkMass(link)); + } + } + + // apply generalized forces + if (btmb->hasFixedBase()) + { + q_index = 0; + } + else + { + btVector3 base_force; + base_force = btVector3(joint_forces[3], joint_forces[4], joint_forces[5]); + + btVector3 base_moment; + base_moment = btVector3(joint_forces[0], joint_forces[1], joint_forces[2]); + + btmb->addBaseForce(world_T_base * base_force); + btmb->addBaseTorque(world_T_base * base_moment); + if (verbose) + { + printf("base force from id: %f %f %f\n", joint_forces[3], joint_forces[4], + joint_forces[5]); + printf("base moment from id: %f %f %f\n", joint_forces[0], joint_forces[1], + joint_forces[2]); + } + q_index = 6; + } + + for (int l = 0; l < btmb->getNumLinks(); l++) + { + const btMultibodyLink &link = btmb->getLink(l); + if (link.m_posVarCount == 1) + { + if (verbose) + { + printf("id:joint_force[%d]= %f, applied to link %d\n", q_index, + joint_forces[q_index], l); + } + btmb->addJointTorque(l, joint_forces[q_index]); + q_index++; + } + } + + // sanity check + if (q_index != q.size()) + { + bt_id_error_message("error in number of dofs for btMultibody and MultiBodyTree\n"); + return -1; + } + + // run forward kinematics & forward dynamics + btAlignedObjectArray world_to_local; + btAlignedObjectArray local_origin; + btmb->forwardKinematics(world_to_local, local_origin); + btmb->computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass, false, false); + + // read generalized accelerations back from btMultiBody + // the mapping from scratch variables to accelerations is taken from the implementation + // of stepVelocitiesMultiDof + btScalar *base_accel = &scratch_r[btmb->getNumDofs()]; + btScalar *joint_accel = base_accel + 6; + btVector3 * spatVel = &scratch_v[0]; + int linkNum = btmb->getNumLinks(); + btVector3 * spatAcc = &scratch_v[0] + (linkNum * 2 *3 + 2 + 2 ) * 1; + if (verbose) + { + printf("linkNum = %d nomdofs=%d\n", linkNum, btmb->getNumDofs()); + } + *acc_error = 0; + int dot_u_offset = 0; + if (btmb->hasFixedBase()) + { + dot_u_offset = 0; + } + else + { + dot_u_offset = 6; + } + + if (true == btmb->hasFixedBase()) + { + for (int i = 0; i < btmb->getNumDofs(); i++) + { + if (verbose) + { + printf("bt:ddot_q[%d]= %.20f, id:ddot_q= %.20f, diff= %.20f\n", i, joint_accel[i], + dot_u[i + dot_u_offset], joint_accel[i] - dot_u[i]); + } + *acc_error += BT_ID_POW(joint_accel[i] - dot_u[i + dot_u_offset], 2); + } + } + else + { + btVector3 base_dot_omega; + btVector3 world_dot_omega; + world_dot_omega = btVector3(base_accel[0], base_accel[1], base_accel[2]); + base_dot_omega = world_T_base.transpose() * world_dot_omega; + + // com happens to coincide with link origin here. If that changes, we need to calculate + // ddot_com + btVector3 base_ddot_com; + btVector3 world_ddot_com; + world_ddot_com = btVector3(base_accel[3], base_accel[4], base_accel[5]); + base_ddot_com = world_T_base.transpose() * world_ddot_com; + + for (int i = 0; i < 3; i++) + { + if (verbose) + { + printf("bt::base_dot_omega(%d)= %e dot_u[%d]= %e, diff= %e\n", i, ((btScalar*)base_dot_omega)[i], + i, dot_u[i], ((btScalar*)base_dot_omega)[i] - dot_u[i]); + } + *acc_error += BT_ID_POW(((btScalar*)base_dot_omega)[i] - dot_u[i], 2); + } + for (int i = 0; i < 3; i++) + { + if (verbose) + { + printf("bt::base_ddot_com(%d)= %e dot_u[%d]= %e, diff= %e\n", i, ((btScalar*)base_ddot_com)[i], + i, dot_u[i + 3], ((btScalar*)base_ddot_com)[i] - dot_u[i + 3]); + } + *acc_error += BT_ID_POW(((btScalar*)base_ddot_com)[i] - dot_u[i + 3], 2); + } + + for (int i = 0; i < btmb->getNumDofs(); i++) + { + if (verbose) + { + printf("bt:ddot_q[%d]= %.10f, id:ddot_q= %e, diff= %e\n", i, joint_accel[i], + dot_u[i + 6], joint_accel[i] - dot_u[i + 6]); + } + *acc_error += BT_ID_POW(joint_accel[i] - dot_u[i + 6], 2); + } + } + *acc_error = std::sqrt(*acc_error); + if (verbose) + { + printf("======dynamics-err: %e\n", *acc_error); + } + + return 0; +} + +/// this test loads the a urdf model with fixed, floating, prismatic and rotational joints, +/// converts in to an inverse dynamics model and compares forward to inverse dynamics for +/// random input +TEST(DynCompareBulletAndRbdl, UrdfPantilt) +{ + MyBtMultiBodyFromURDF mb_load(gravity, kBaseFixed); + + char relativeFileName[1024]; + + ASSERT_TRUE(b3ResourcePath::findResourcePath(kUrdfFile, relativeFileName, 1024,0)); + + mb_load.setFileName(relativeFileName); + mb_load.setFlag(URDF_USE_INERTIA_FROM_FILE); + mb_load.init(); + + btMultiBody *btmb = mb_load.getBtMultiBody(); + int numDofs = btmb->getNumDofs(); + + btAlignedObjectArray q; + btAlignedObjectArray u; + btAlignedObjectArray dot_u; + btAlignedObjectArray joint_forces; + q.resizeNoInitialize(numDofs); + u.resizeNoInitialize(numDofs); + dot_u.resizeNoInitialize(numDofs); + joint_forces.resizeNoInitialize(numDofs); + + const int kNLoops = 5; + double max_pos_error = 0; + double max_acc_error = 0; + + b3Srand(0); + + RigidBodyDynamics::Model model; + + if (!RigidBodyDynamics::Addons::URDFReadFromFile(relativeFileName, &model, floatbase, verbose)) { + printf("Loading of urdf model failed!\n"); + return; + } + + model.gravity = Vector3d (0.,0., -10.); + + for (int loop = 0; loop < kNLoops; loop++) + { + for (int i = 0; i < q.size(); i++) + { + q[i] = b3RandRange(-B3_PI, B3_PI); + u[i] = b3RandRange(-B3_PI, B3_PI); + joint_forces[i] = b3RandRange(0, 20); + } + // Initialization of the input vectors + VectorNd Q = VectorNd::Constant((size_t)model.dof_count, 0.); + VectorNd QDot = VectorNd::Constant((size_t)model.dof_count, 0.); + VectorNd QDDot = VectorNd::Constant((size_t)model.dof_count, 0.); + VectorNd Tau = VectorNd::Constant((size_t)model.dof_count, 0.); + for (int i = 0; i < q.size(); i++) + { + Q[i] = q[i]; + QDot[i] = u[i]; + Tau[i] = joint_forces[i]; + } + ForwardDynamics(model, Q, QDot, Tau, QDDot); + unsigned int i; + + for (int i = 0; i < q.size(); i++) + { + dot_u[i] = QDDot[i]; + } + double acc_error; + btmb->clearForcesAndTorques(); + EXPECT_EQ(compareABAForwardDynamicsBulletAndRbdl(q, u, joint_forces, dot_u, gravity, FLAGS_verbose, btmb, + &acc_error), + 0); + if (acc_error > max_acc_error) + { + max_acc_error = acc_error; + } + } + + if (FLAGS_verbose) + { + printf("max_acc_error= %e\n", max_acc_error); + } + + EXPECT_LT(max_acc_error, std::numeric_limits::epsilon() * 1e5); +} + +int main(int argc, char **argv) +{ + b3CommandLineArgs myArgs(argc, argv); + FLAGS_verbose = myArgs.CheckCmdLineFlag("verbose"); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}