TorqueGameEngines/Torque3D
1
0
Fork 0
mirror of https://github.com/TorqueGameEngines/Torque3D.git synced 2026-07-13 23:54:35 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions 3

* Adjustment: Update Bullet version to 3.24.

Browse source
This commit is contained in:
Robert MacGregor 2022-06-27 10:01:08 -04:00
parent 35de012ee7
commit 4a3f31df2a
6148 changed files with 2112532 additions and 56873 deletions
Download patch file Download diff file Expand all files Collapse all files

198
Engine/lib/bullet/test/InverseDynamics/CMakeLists.txt Normal file
View file

@ -0,0 +1,198 @@
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)
FIND_PACKAGE(Threads)
LINK_LIBRARIES( ${CMAKE_THREAD_LIBS_INIT} )
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(
.
../../src
../gtest-1.7.0/include
../../Extras/InverseDynamics
)
#ADD_DEFINITIONS(-DGTEST_HAS_PTHREAD=1)
ADD_DEFINITIONS(-D_VARIADIC_MAX=10)
LINK_LIBRARIES(
BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest
)
IF (NOT WIN32)
FIND_PACKAGE(Threads)
LINK_LIBRARIES( ${CMAKE_THREAD_LIBS_INIT} )
ENDIF()
ADD_EXECUTABLE(Test_BulletInverseDynamicsJacobian
test_invdyn_jacobian.cpp
)
ADD_TEST(Test_BulletInverseDynamicsJacobian_PASS Test_BulletInverseDynamicsJacobian)
IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
SET_TARGET_PROPERTIES(Test_BulletInverseDynamicsJacobian PROPERTIES DEBUG_POSTFIX "_Debug")
SET_TARGET_PROPERTIES(Test_BulletInverseDynamicsJacobian PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel")
SET_TARGET_PROPERTIES(Test_BulletInverseDynamicsJacobian PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo")
ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
INCLUDE_DIRECTORIES(
.
../../src
../gtest-1.7.0/include
../../Extras/InverseDynamics
../../examples/ThirdPartyLibs
)
#ADD_DEFINITIONS(-DGTEST_HAS_PTHREAD=1)
ADD_DEFINITIONS(-D_VARIADIC_MAX=10)
LINK_LIBRARIES(
BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest
)
IF (NOT WIN32)
FIND_PACKAGE(Threads)
LINK_LIBRARIES( ${CMAKE_THREAD_LIBS_INIT} )
ENDIF()
ADD_EXECUTABLE(Test_BulletInverseForwardDynamics
test_invdyn_bullet.cpp
../../examples/Utils/b3ResourcePath.cpp
../../examples/Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h
../../examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.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_BulletInverseForwardDynamics_PASS Test_BulletInverseForwardDynamics)
IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
SET_TARGET_PROPERTIES(Test_BulletInverseForwardDynamics PROPERTIES DEBUG_POSTFIX "_Debug")
SET_TARGET_PROPERTIES(Test_BulletInverseForwardDynamics PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel")
SET_TARGET_PROPERTIES(Test_BulletInverseForwardDynamics PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo")
ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
INCLUDE_DIRECTORIES(
.
../../src
../gtest-1.7.0/include
../../Extras/InverseDynamics
)
#ADD_DEFINITIONS(-DGTEST_HAS_PTHREAD=1)
ADD_DEFINITIONS(-D_VARIADIC_MAX=10)
LINK_LIBRARIES(
BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest
)
IF (NOT WIN32)
FIND_PACKAGE(Threads)
LINK_LIBRARIES( ${CMAKE_THREAD_LIBS_INIT} )
ENDIF()
ADD_EXECUTABLE(Test_BulletInverseDynamics
test_invdyn_kinematics.cpp
)
ADD_TEST(Test_BulletInverseDynamics_PASS Test_BulletInverseDynamics)
IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
SET_TARGET_PROPERTIES(Test_BulletInverseDynamics PROPERTIES DEBUG_POSTFIX "_Debug")
SET_TARGET_PROPERTIES(Test_BulletInverseDynamics PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel")
SET_TARGET_PROPERTIES(Test_BulletInverseDynamics PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo")
ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)

136
Engine/lib/bullet/test/InverseDynamics/premake4.lua Normal file
View file

@ -0,0 +1,136 @@
project "Test_InverseDynamicsKinematics"
kind "ConsoleApp"
-- defines { }
includedirs
{
".",
"../../src",
"../../examples/InverseDynamics",
"../../Extras/InverseDynamics",
"../gtest-1.7.0/include"
}
if os.is("Windows") then
--see http://stackoverflow.com/questions/12558327/google-test-in-visual-studio-2012
defines {"_VARIADIC_MAX=10"}
end
links {"BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common","LinearMath", "gtest"}
files {
"test_invdyn_kinematics.cpp",
}
if os.is("Linux") then
links {"pthread"}
end
project "Test_InverseDynamicsJacobian"
kind "ConsoleApp"
-- defines { }
includedirs
{
".",
"../../src",
"../../examples/InverseDynamics",
"../../Extras/InverseDynamics",
"../gtest-1.7.0/include"
}
if os.is("Windows") then
--see http://stackoverflow.com/questions/12558327/google-test-in-visual-studio-2012
defines {"_VARIADIC_MAX=10"}
end
links {"BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common","LinearMath", "gtest"}
files {
"test_invdyn_jacobian.cpp",
}
if os.is("Linux") then
links {"pthread"}
end
project "Test_InverseForwardDynamics"
kind "ConsoleApp"
-- defines { }
includedirs
{
".",
"../../src",
"../../examples/InverseDynamics",
"../../examples/ThirdPartyLibs",
"../../Extras/InverseDynamics",
"../gtest-1.7.0/include",
"../../examples/ThirdPartyLibs",
}
if os.is("Windows") then
--see http://stackoverflow.com/questions/12558327/google-test-in-visual-studio-2012
defines {"_VARIADIC_MAX=10"}
end
links {"BulletDynamics", "BulletCollision","BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common","LinearMath", "gtest"}
files {
"test_invdyn_bullet.cpp",
"../../examples/Utils/b3ResourcePath.cpp",
"../../examples/Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h",
"../../examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.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/*",
"../../Extras/Serialize/BulletFileLoader/*",
"../../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",
"../../examples/ThirdPartyLibs/urdf/boost_replacement/lexical_cast.h",
}
if os.is("Linux") then
links {"pthread"}
end

376
Engine/lib/bullet/test/InverseDynamics/test_dynABA_bulletVSrbdl.cpp Normal file
View file

@ -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 <cmath>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <string>
#include <btBulletDynamicsCommon.h>
#include <BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h>
#include <BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h>
#include <BulletDynamics/Featherstone/btMultiBodyPoint2Point.h>
#include <BulletDynamics/Featherstone/btMultiBodyLinkCollider.h>
#include <gtest/gtest.h>
#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 <btMultiBodyFromURDF.hpp>
#include <MultiBodyTreeDebugGraph.hpp>
#include "Bullet3Common/b3CommandLineArgs.h"
#include "Bullet3Common/b3Random.h"
#include <rbdl/rbdl.h>
#include <rbdl/rbdl_utils.h>
#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<btScalar> &q, btAlignedObjectArray<btScalar> &u, btAlignedObjectArray<btScalar> &joint_forces, btAlignedObjectArray<btScalar> &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<btScalar> scratch_r;
btAlignedObjectArray<btVector3> scratch_v;
btAlignedObjectArray<btMatrix3x3> 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<btQuaternion> world_to_local;
btAlignedObjectArray<btVector3> 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<btScalar> q;
btAlignedObjectArray<btScalar> u;
btAlignedObjectArray<btScalar> dot_u;
btAlignedObjectArray<btScalar> 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<idScalar>::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();
}

120
Engine/lib/bullet/test/InverseDynamics/test_invdyn_bullet.cpp Normal file
View file

@ -0,0 +1,120 @@
/// create a bullet btMultiBody model of a tree structured multibody system,
/// convert that model to a MultiBodyTree model.
/// Then - run inverse dynamics on random input data (q, u, dot_u) to get forces
/// - run forward dynamics on (q,u, forces) to get accelerations
/// - compare input accelerations to inverse dynamics to output from forward dynamics
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <string>
#include <btBulletDynamicsCommon.h>
#include <btMultiBodyTreeCreator.hpp>
#include <BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h>
#include <BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h>
#include <BulletDynamics/Featherstone/btMultiBodyPoint2Point.h>
#include <BulletDynamics/Featherstone/btMultiBodyLinkCollider.h>
#include <gtest/gtest.h>
#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 <invdyn_bullet_comparison.hpp>
#include <btMultiBodyFromURDF.hpp>
#include <MultiBodyTreeCreator.hpp>
#include <MultiBodyTreeDebugGraph.hpp>
#include "Bullet3Common/b3CommandLineArgs.h"
#include "Bullet3Common/b3Random.h"
using namespace btInverseDynamics;
bool FLAGS_verbose = false;
static btVector3 gravity(0, 0, -10);
static const bool kBaseFixed = false;
static const char kUrdfFile[] = "r2d2.urdf";
/// 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(InvDynCompare, bulletUrdfR2D2)
{
MyBtMultiBodyFromURDF mb_load(gravity, kBaseFixed);
char relativeFileName[1024];
ASSERT_TRUE(b3ResourcePath::findResourcePath(kUrdfFile, relativeFileName, 1024,0));
mb_load.setFileName(relativeFileName);
mb_load.init();
btMultiBodyTreeCreator id_creator;
btMultiBody *btmb = mb_load.getBtMultiBody();
ASSERT_EQ(id_creator.createFromBtMultiBody(btmb), 0);
MultiBodyTree *id_tree = CreateMultiBodyTree(id_creator);
ASSERT_EQ(0x0 != id_tree, true);
vecx q(id_tree->numDoFs());
vecx u(id_tree->numDoFs());
vecx dot_u(id_tree->numDoFs());
vecx joint_forces(id_tree->numDoFs());
const int kNLoops = 10;
double max_pos_error = 0;
double max_acc_error = 0;
b3Srand(0);
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);
dot_u(i) = b3RandRange(-B3_PI, B3_PI);
}
double pos_error;
double acc_error;
btmb->clearForcesAndTorques();
id_tree->clearAllUserForcesAndMoments();
// call inverse dynamics once, to get global position & velocity of root body
// (fixed, so q, u, dot_u arbitrary)
EXPECT_EQ(id_tree->calculateInverseDynamics(q, u, dot_u, &joint_forces), 0);
EXPECT_EQ(compareInverseAndForwardDynamics(q, u, dot_u, gravity, FLAGS_verbose, btmb, id_tree,
&pos_error, &acc_error),
0);
if (pos_error > max_pos_error)
{
max_pos_error = pos_error;
}
if (acc_error > max_acc_error)
{
max_acc_error = acc_error;
}
}
if (FLAGS_verbose)
{
printf("max_pos_error= %e\n", max_pos_error);
printf("max_acc_error= %e\n", max_acc_error);
}
EXPECT_LT(max_pos_error, std::numeric_limits<idScalar>::epsilon() * 1e4);
EXPECT_LT(max_acc_error, std::numeric_limits<idScalar>::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();
}

361
Engine/lib/bullet/test/InverseDynamics/test_invdyn_jacobian.cpp Normal file
View file

@ -0,0 +1,361 @@
// Test of kinematic consistency: check if finite differences of velocities, accelerations
// match positions
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <iostream>
#include <gtest/gtest.h>
#include "Bullet3Common/b3Random.h"
#include "CloneTreeCreator.hpp"
#include "CoilCreator.hpp"
#include "DillCreator.hpp"
#include "RandomTreeCreator.hpp"
#include "BulletInverseDynamics/MultiBodyTree.hpp"
#include "MultiBodyTreeDebugGraph.hpp"
using namespace btInverseDynamics;
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
// minimal smart pointer to make this work for c++2003
template <typename T>
class ptr
{
ptr();
ptr(const ptr&);
public:
ptr(T* p) : m_p(p){};
~ptr() { delete m_p; }
T& operator*() { return *m_p; }
T* operator->() { return m_p; }
T* get() { return m_p; }
const T* get() const { return m_p; }
friend bool operator==(const ptr<T>& lhs, const ptr<T>& rhs) { return rhs.m_p == lhs.m_p; }
friend bool operator!=(const ptr<T>& lhs, const ptr<T>& rhs)
{
return !(rhs.m_p == lhs.m_p);
}
private:
T* m_p;
};
void calculateDotJacUError(const MultiBodyTreeCreator& creator, const int nloops,
double* max_error)
{
// tree1 is used as reference to compute dot(Jacobian)*u from acceleration(dot(u)=0)
ptr<MultiBodyTree> tree1(CreateMultiBodyTree(creator));
ASSERT_TRUE(0x0 != tree1);
CloneTreeCreator clone(tree1.get());
// tree2 is used to compute dot(Jacobian)*u using the calculateJacobian function
ptr<MultiBodyTree> tree2(CreateMultiBodyTree(clone));
ASSERT_TRUE(0x0 != tree2);
const int ndofs = tree1->numDoFs();
const int nbodies = tree1->numBodies();
if (ndofs <= 0)
{
*max_error = 0;
return;
}
vecx q(ndofs);
vecx u(ndofs);
vecx dot_u(ndofs);
vecx zero(ndofs);
setZero(zero);
double max_lin_error = 0;
double max_ang_error = 0;
for (int loop = 0; loop < nloops; loop++)
{
for (int i = 0; i < q.size(); i++)
{
q(i) = b3RandRange(-B3_PI, B3_PI);
u(i) = b3RandRange(-B3_PI, B3_PI);
}
EXPECT_EQ(0, tree1->calculateKinematics(q, u, zero));
EXPECT_EQ(0, tree2->calculatePositionAndVelocityKinematics(q, u));
EXPECT_EQ(0, tree2->calculateJacobians(q, u));
for (int idx = 0; idx < nbodies; idx++)
{
vec3 tmp1, tmp2;
vec3 diff;
EXPECT_EQ(0, tree1->getBodyLinearAcceleration(idx, &tmp1));
EXPECT_EQ(0, tree2->getBodyDotJacobianTransU(idx, &tmp2));
diff = tmp1 - tmp2;
double lin_error = maxAbs(diff);
if (lin_error > max_lin_error)
{
max_lin_error = lin_error;
}
EXPECT_EQ(0, tree1->getBodyAngularAcceleration(idx, &tmp1));
EXPECT_EQ(0, tree2->getBodyDotJacobianRotU(idx, &tmp2));
diff = tmp1 - tmp2;
double ang_error = maxAbs(diff);
if (ang_error > max_ang_error)
{
max_ang_error = ang_error;
}
}
}
*max_error = max_ang_error > max_lin_error ? max_ang_error : max_lin_error;
}
void calculateJacobianError(const MultiBodyTreeCreator& creator, const int nloops,
double* max_error)
{
// tree1 is used as reference to compute the Jacobian from velocities with unit u vectors.
ptr<MultiBodyTree> tree1(CreateMultiBodyTree(creator));
ASSERT_TRUE(0x0 != tree1);
// tree2 is used to compute the Jacobians using the calculateJacobian function
CloneTreeCreator clone(tree1.get());
ptr<MultiBodyTree> tree2(CreateMultiBodyTree(clone));
ASSERT_TRUE(0x0 != tree2);
const int ndofs = tree1->numDoFs();
const int nbodies = tree1->numBodies();
if (ndofs <= 0)
{
*max_error = 0;
return;
}
vecx q(ndofs);
vecx zero(ndofs);
setZero(zero);
vecx one(ndofs);
double max_lin_error = 0;
double max_ang_error = 0;
for (int loop = 0; loop < nloops; loop++)
{
for (int i = 0; i < q.size(); i++)
{
q(i) = b3RandRange(-B3_PI, B3_PI);
}
EXPECT_EQ(0, tree2->calculatePositionKinematics(q));
EXPECT_EQ(0, tree2->calculateJacobians(q));
for (int idx = 0; idx < nbodies; idx++)
{
mat3x ref_jac_r(3, ndofs);
mat3x ref_jac_t(3, ndofs);
ref_jac_r.setZero();
ref_jac_t.setZero();
// this re-computes all jacobians for every body ...
// but avoids std::vector<eigen matrix> issues
for (int col = 0; col < ndofs; col++)
{
setZero(one);
one(col) = 1.0;
EXPECT_EQ(0, tree1->calculatePositionAndVelocityKinematics(q, one));
vec3 vel, omg;
EXPECT_EQ(0, tree1->getBodyLinearVelocity(idx, &vel));
EXPECT_EQ(0, tree1->getBodyAngularVelocity(idx, &omg));
setMat3xElem(0, col, omg(0), &ref_jac_r);
setMat3xElem(1, col, omg(1), &ref_jac_r);
setMat3xElem(2, col, omg(2), &ref_jac_r);
setMat3xElem(0, col, vel(0), &ref_jac_t);
setMat3xElem(1, col, vel(1), &ref_jac_t);
setMat3xElem(2, col, vel(2), &ref_jac_t);
}
mat3x jac_r(3, ndofs);
mat3x jac_t(3, ndofs);
mat3x diff(3, ndofs);
EXPECT_EQ(0, tree2->getBodyJacobianTrans(idx, &jac_t));
EXPECT_EQ(0, tree2->getBodyJacobianRot(idx, &jac_r));
sub(ref_jac_t, jac_t, &diff);
double lin_error = maxAbsMat3x(diff);
if (lin_error > max_lin_error)
{
max_lin_error = lin_error;
}
sub(ref_jac_r, jac_r, &diff);
double ang_error = maxAbsMat3x(diff);
if (ang_error > max_ang_error)
{
max_ang_error = ang_error;
}
}
}
*max_error = max_ang_error > max_lin_error ? max_ang_error : max_lin_error;
}
void calculateVelocityJacobianError(const MultiBodyTreeCreator& creator, const int nloops,
double* max_error)
{
// tree1 is used as reference to compute the velocities directly
ptr<MultiBodyTree> tree1(CreateMultiBodyTree(creator));
ASSERT_TRUE(0x0 != tree1);
// tree2 is used to compute the velocities via jacobians
CloneTreeCreator clone(tree1.get());
ptr<MultiBodyTree> tree2(CreateMultiBodyTree(clone));
ASSERT_TRUE(0x0 != tree2);
const int ndofs = tree1->numDoFs();
const int nbodies = tree1->numBodies();
if (ndofs <= 0)
{
*max_error = 0;
return;
}
vecx q(ndofs);
vecx u(ndofs);
double max_lin_error = 0;
double max_ang_error = 0;
for (int loop = 0; loop < nloops; loop++)
{
for (int i = 0; i < q.size(); i++)
{
q(i) = b3RandRange(-B3_PI, B3_PI);
u(i) = b3RandRange(-B3_PI, B3_PI);
}
EXPECT_EQ(0, tree1->calculatePositionAndVelocityKinematics(q, u));
EXPECT_EQ(0, tree2->calculatePositionKinematics(q));
EXPECT_EQ(0, tree2->calculateJacobians(q));
for (int idx = 0; idx < nbodies; idx++)
{
vec3 vel1;
vec3 omg1;
vec3 vel2;
vec3 omg2;
mat3x jac_r2(3, ndofs);
mat3x jac_t2(3, ndofs);
EXPECT_EQ(0, tree1->getBodyLinearVelocity(idx, &vel1));
EXPECT_EQ(0, tree1->getBodyAngularVelocity(idx, &omg1));
EXPECT_EQ(0, tree2->getBodyJacobianTrans(idx, &jac_t2));
EXPECT_EQ(0, tree2->getBodyJacobianRot(idx, &jac_r2));
omg2 = jac_r2 * u;
vel2 = jac_t2 * u;
double lin_error = maxAbs(vel1 - vel2);
if (lin_error > max_lin_error)
{
max_lin_error = lin_error;
}
double ang_error = maxAbs(omg1 - omg2);
if (ang_error > max_ang_error)
{
max_ang_error = ang_error;
}
}
}
*max_error = max_ang_error > max_lin_error ? max_ang_error : max_lin_error;
}
// test nonlinear terms: dot(Jacobian)*u (linear and angular acceleration for dot_u ==0)
// from Jacobian calculation method and pseudo-numerically using via the kinematics method.
TEST(InvDynJacobians, JacDotJacU)
{
const int kNumLevels = 5;
#ifdef B3_USE_DOUBLE_PRECISION
const double kMaxError = 1e-12;
#else
const double kMaxError = 5e-5;
#endif
const int kNumLoops = 20;
for (int level = 0; level < kNumLevels; level++)
{
const int nbodies = BT_ID_POW(2, level);
CoilCreator coil(nbodies);
double error;
calculateDotJacUError(coil, kNumLoops, &error);
EXPECT_GT(kMaxError, error);
DillCreator dill(level);
calculateDotJacUError(dill, kNumLoops, &error);
EXPECT_GT(kMaxError, error);
}
const int kRandomLoops = 100;
const int kMaxRandomBodies = 128;
for (int loop = 0; loop < kRandomLoops; loop++)
{
RandomTreeCreator random(kMaxRandomBodies);
double error;
calculateDotJacUError(random, kNumLoops, &error);
EXPECT_GT(kMaxError, error);
}
}
// Jacobians: linear and angular acceleration for dot_u ==0
// from Jacobian calculation method and pseudo-numerically using via the kinematics method.
TEST(InvDynJacobians, Jacobians)
{
const int kNumLevels = 5;
#ifdef B3_USE_DOUBLE_PRECISION
const double kMaxError = 1e-12;
#else
const double kMaxError = 5e-5;
#endif
const int kNumLoops = 20;
for (int level = 0; level < kNumLevels; level++)
{
const int nbodies = BT_ID_POW(2, level);
CoilCreator coil(nbodies);
double error;
calculateJacobianError(coil, kNumLoops, &error);
EXPECT_GT(kMaxError, error);
DillCreator dill(level);
calculateDotJacUError(dill, kNumLoops, &error);
EXPECT_GT(kMaxError, error);
}
const int kRandomLoops = 20;
const int kMaxRandomBodies = 16;
for (int loop = 0; loop < kRandomLoops; loop++)
{
RandomTreeCreator random(kMaxRandomBodies);
double error;
calculateJacobianError(random, kNumLoops, &error);
EXPECT_GT(kMaxError, error);
}
}
// test for jacobian*u == velocity
TEST(InvDynJacobians, VelocitiesFromJacobians)
{
const int kRandomLoops = 20;
const int kMaxRandomBodies = 16;
const int kNumLoops = 20;
#ifdef B3_USE_DOUBLE_PRECISION
const double kMaxError = 1e-12;
#else
const double kMaxError = 5e-5;
#endif
for (int loop = 0; loop < kRandomLoops; loop++)
{
RandomTreeCreator random(kMaxRandomBodies);
double error;
calculateVelocityJacobianError(random, kNumLoops, &error);
EXPECT_GT(kMaxError, error);
}
}
#endif
int main(int argc, char** argv)
{
b3Srand(1234);
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

404
Engine/lib/bullet/test/InverseDynamics/test_invdyn_kinematics.cpp Normal file
View file

@ -0,0 +1,404 @@
// Test of kinematic consistency: check if finite differences of velocities, accelerations
// match positions
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <iostream>
#include <gtest/gtest.h>
#include "../Extras/InverseDynamics/CoilCreator.hpp"
#include "../Extras/InverseDynamics/DillCreator.hpp"
#include "../Extras/InverseDynamics/SimpleTreeCreator.hpp"
#include "BulletInverseDynamics/MultiBodyTree.hpp"
using namespace btInverseDynamics;
const int kLevel = 5;
const int kNumBodies = BT_ID_POW(2, kLevel);
// template function for calculating the norm
template <typename T>
idScalar calculateNorm(T&);
// only implemented for vec3
template <>
idScalar calculateNorm(vec3& v)
{
return BT_ID_SQRT(BT_ID_POW(v(0), 2) + BT_ID_POW(v(1), 2) + BT_ID_POW(v(2), 2));
}
// template function to convert a DiffType (finite differences)
// to a ValueType. This is for angular velocity calculations
// via finite differences.
template <typename ValueType, typename DiffType>
DiffType toDiffType(ValueType& fd, ValueType& val);
// vector case: just return finite difference approximation
template <>
vec3 toDiffType(vec3& fd, vec3& val)
{
return fd;
}
// orientation case: calculate spin tensor and extract angular velocity
template <>
vec3 toDiffType(mat33& fd, mat33& val)
{
// spin tensor
mat33 omega_tilde = fd * val.transpose();
// extract vector from spin tensor
vec3 omega;
omega(0) = 0.5 * (omega_tilde(2, 1) - omega_tilde(1, 2));
omega(1) = 0.5 * (omega_tilde(0, 2) - omega_tilde(2, 0));
omega(2) = 0.5 * (omega_tilde(1, 0) - omega_tilde(0, 1));
return omega;
}
/// Class for calculating finite difference approximation
/// of time derivatives and comparing it to an analytical solution
/// DiffType and ValueType can be different, to allow comparison
/// of angular velocity vectors and orientations given as transform matrices.
template <typename ValueType, typename DiffType>
class DiffFD
{
public:
DiffFD() : m_dt(0.0), m_num_updates(0), m_max_error(0.0), m_max_value(0.0), m_valid_fd(false) {}
void init(std::string name, idScalar dt)
{
m_name = name;
m_dt = dt;
m_num_updates = 0;
m_max_error = 0.0;
m_max_value = 0.0;
m_valid_fd = false;
}
void update(const ValueType& val, const DiffType& true_diff)
{
m_val = val;
if (m_num_updates > 2)
{
// 2nd order finite difference approximation for d(value)/dt
ValueType diff_value_fd = (val - m_older_val) / (2.0 * m_dt);
// convert to analytical diff type. This is for angular velocities
m_diff_fd = toDiffType<ValueType, DiffType>(diff_value_fd, m_old_val);
// now, calculate the error
DiffType error_value_type = m_diff_fd - m_old_true_diff;
idScalar error = calculateNorm<DiffType>(error_value_type);
if (error > m_max_error)
{
m_max_error = error;
}
idScalar value = calculateNorm<DiffType>(m_old_true_diff);
if (value > m_max_value)
{
m_max_value = value;
}
m_valid_fd = true;
}
m_older_val = m_old_val;
m_old_val = m_val;
m_old_true_diff = true_diff;
m_num_updates++;
m_time += m_dt;
}
void printMaxError()
{
printf("max_error: %e dt= %e max_value= %e fraction= %e\n", m_max_error, m_dt, m_max_value,
m_max_value > 0.0 ? m_max_error / m_max_value : 0.0);
}
void printCurrent()
{
if (m_valid_fd)
{
// note: m_old_true_diff already equals m_true_diff here, so values are not aligned.
// (but error calculation takes this into account)
printf("%s time: %e fd: %e %e %e true: %e %e %e\n", m_name.c_str(), m_time,
m_diff_fd(0), m_diff_fd(1), m_diff_fd(2), m_old_true_diff(0), m_old_true_diff(1),
m_old_true_diff(2));
}
}
idScalar getMaxError() const { return m_max_error; }
idScalar getMaxValue() const { return m_max_value; }
private:
idScalar m_dt;
ValueType m_val;
ValueType m_old_val;
ValueType m_older_val;
DiffType m_old_true_diff;
DiffType m_diff_fd;
int m_num_updates;
idScalar m_max_error;
idScalar m_max_value;
idScalar m_time;
std::string m_name;
bool m_valid_fd;
};
template <typename ValueType, typename DiffType>
class VecDiffFD
{
public:
VecDiffFD(std::string name, int dim, idScalar dt) : m_name(name), m_fd(dim), m_dt(dt)
{
for (int i = 0; i < m_fd.size(); i++)
{
char buf[256];
BT_ID_SNPRINTF(buf, 256, "%s-%.2d", name.c_str(), i);
m_fd[i].init(buf, dt);
}
}
void update(int i, ValueType& val, DiffType& true_diff) { m_fd[i].update(val, true_diff); }
idScalar getMaxError() const
{
idScalar max_error = 0;
for (int i = 0; i < m_fd.size(); i++)
{
const idScalar error = m_fd[i].getMaxError();
if (error > max_error)
{
max_error = error;
}
}
return max_error;
}
idScalar getMaxValue() const
{
idScalar max_value = 0;
for (int i = 0; i < m_fd.size(); i++)
{
const idScalar value = m_fd[i].getMaxValue();
if (value > max_value)
{
max_value = value;
}
}
return max_value;
}
void printMaxError()
{
printf("%s: total dt= %e max_error= %e\n", m_name.c_str(), m_dt, getMaxError());
}
void printCurrent()
{
for (int i = 0; i < m_fd.size(); i++)
{
m_fd[i].printCurrent();
}
}
private:
std::string m_name;
std::vector<DiffFD<ValueType, DiffType> > m_fd;
const idScalar m_dt;
idScalar m_max_error;
};
// calculate maximum difference between finite difference and analytical differentiation
int calculateDifferentiationError(const MultiBodyTreeCreator& creator, idScalar deltaT,
idScalar endTime, idScalar* max_linear_velocity_error,
idScalar* max_angular_velocity_error,
idScalar* max_linear_acceleration_error,
idScalar* max_angular_acceleration_error)
{
// setup system
MultiBodyTree* tree = CreateMultiBodyTree(creator);
if (0x0 == tree)
{
return -1;
}
// set gravity to zero, so nothing is added to accelerations in forward kinematics
vec3 gravity_zero;
gravity_zero(0) = 0;
gravity_zero(1) = 0;
gravity_zero(2) = 0;
tree->setGravityInWorldFrame(gravity_zero);
//
const idScalar kAmplitude = 1.0;
const idScalar kFrequency = 1.0;
vecx q(tree->numDoFs());
vecx dot_q(tree->numDoFs());
vecx ddot_q(tree->numDoFs());
vecx joint_forces(tree->numDoFs());
VecDiffFD<vec3, vec3> fd_vel("linear-velocity", tree->numBodies(), deltaT);
VecDiffFD<vec3, vec3> fd_acc("linear-acceleration", tree->numBodies(), deltaT);
VecDiffFD<mat33, vec3> fd_omg("angular-velocity", tree->numBodies(), deltaT);
VecDiffFD<vec3, vec3> fd_omgd("angular-acceleration", tree->numBodies(), deltaT);
for (idScalar t = 0.0; t < endTime; t += deltaT)
{
for (int body = 0; body < tree->numBodies(); body++)
{
q(body) = kAmplitude * sin(t * 2.0 * BT_ID_PI * kFrequency);
dot_q(body) = kAmplitude * 2.0 * BT_ID_PI * kFrequency * cos(t * 2.0 * BT_ID_PI * kFrequency);
ddot_q(body) =
-kAmplitude * pow(2.0 * BT_ID_PI * kFrequency, 2) * sin(t * 2.0 * BT_ID_PI * kFrequency);
}
if (-1 == tree->calculateInverseDynamics(q, dot_q, ddot_q, &joint_forces))
{
delete tree;
return -1;
}
// position/velocity
for (int body = 0; body < tree->numBodies(); body++)
{
vec3 pos;
vec3 vel;
mat33 world_T_body;
vec3 omega;
vec3 dot_omega;
vec3 acc;
tree->getBodyOrigin(body, &pos);
tree->getBodyTransform(body, &world_T_body);
tree->getBodyLinearVelocity(body, &vel);
tree->getBodyAngularVelocity(body, &omega);
tree->getBodyLinearAcceleration(body, &acc);
tree->getBodyAngularAcceleration(body, &dot_omega);
fd_vel.update(body, pos, vel);
fd_omg.update(body, world_T_body, omega);
fd_acc.update(body, vel, acc);
fd_omgd.update(body, omega, dot_omega);
// fd_vel.printCurrent();
//fd_acc.printCurrent();
//fd_omg.printCurrent();
//fd_omgd.printCurrent();
}
}
*max_linear_velocity_error = fd_vel.getMaxError() / fd_vel.getMaxValue();
*max_angular_velocity_error = fd_omg.getMaxError() / fd_omg.getMaxValue();
*max_linear_acceleration_error = fd_acc.getMaxError() / fd_acc.getMaxValue();
*max_angular_acceleration_error = fd_omgd.getMaxError() / fd_omgd.getMaxValue();
delete tree;
return 0;
}
// first test: absolute difference between numerical and numerial
// differentiation should be small
TEST(InvDynKinematicsDifferentiation, errorAbsolute)
{
//CAVEAT:these values are hand-tuned to work for the specific trajectory defined above.
#ifdef BT_ID_USE_DOUBLE_PRECISION
const idScalar kDeltaT = 1e-7;
const idScalar kAcceptableError = 1e-4;
#else
const idScalar kDeltaT = 1e-4;
const idScalar kAcceptableError = 5e-3;
#endif
const idScalar kDuration = 0.01;
CoilCreator coil_creator(kNumBodies);
DillCreator dill_creator(kLevel);
SimpleTreeCreator simple_creator(kNumBodies);
idScalar max_linear_velocity_error;
idScalar max_angular_velocity_error;
idScalar max_linear_acceleration_error;
idScalar max_angular_acceleration_error;
// test serial chain
calculateDifferentiationError(coil_creator, kDeltaT, kDuration, &max_linear_velocity_error,
&max_angular_velocity_error, &max_linear_acceleration_error,
&max_angular_acceleration_error);
EXPECT_LT(max_linear_velocity_error, kAcceptableError);
EXPECT_LT(max_angular_velocity_error, kAcceptableError);
EXPECT_LT(max_linear_acceleration_error, kAcceptableError);
EXPECT_LT(max_angular_acceleration_error, kAcceptableError);
// test branched tree
calculateDifferentiationError(dill_creator, kDeltaT, kDuration, &max_linear_velocity_error,
&max_angular_velocity_error, &max_linear_acceleration_error,
&max_angular_acceleration_error);
EXPECT_LT(max_linear_velocity_error, kAcceptableError);
EXPECT_LT(max_angular_velocity_error, kAcceptableError);
EXPECT_LT(max_linear_acceleration_error, kAcceptableError);
EXPECT_LT(max_angular_acceleration_error, kAcceptableError);
// test system with different joint types
calculateDifferentiationError(simple_creator, kDeltaT, kDuration, &max_linear_velocity_error,
&max_angular_velocity_error, &max_linear_acceleration_error,
&max_angular_acceleration_error);
EXPECT_LT(max_linear_velocity_error, kAcceptableError);
EXPECT_LT(max_angular_velocity_error, kAcceptableError);
EXPECT_LT(max_linear_acceleration_error, kAcceptableError);
EXPECT_LT(max_angular_acceleration_error, kAcceptableError);
}
// second test: check if the change in the differentiation error
// is consitent with the second order approximation, ie, error ~ O(dt^2)
TEST(InvDynKinematicsDifferentiation, errorOrder)
{
const idScalar kDeltaTs[2] = {1e-4, 1e-5};
const idScalar kDuration = 1e-2;
CoilCreator coil_creator(kNumBodies);
// DillCreator dill_creator(kLevel);
// SimpleTreeCreator simple_creator(kNumBodies);
idScalar max_linear_velocity_error[2];
idScalar max_angular_velocity_error[2];
idScalar max_linear_acceleration_error[2];
idScalar max_angular_acceleration_error[2];
// test serial chain
calculateDifferentiationError(coil_creator, kDeltaTs[0], kDuration,
&max_linear_velocity_error[0], &max_angular_velocity_error[0],
&max_linear_acceleration_error[0],
&max_angular_acceleration_error[0]);
calculateDifferentiationError(coil_creator, kDeltaTs[1], kDuration,
&max_linear_velocity_error[1], &max_angular_velocity_error[1],
&max_linear_acceleration_error[1],
&max_angular_acceleration_error[1]);
/*
const idScalar expected_linear_velocity_error_1 =
max_linear_velocity_error[0] * pow(kDeltaTs[1] / kDeltaTs[0], 2);
const idScalar expected_angular_velocity_error_1 =
max_angular_velocity_error[0] * pow(kDeltaTs[1] / kDeltaTs[0], 2);
const idScalar expected_linear_acceleration_error_1 =
max_linear_acceleration_error[0] * pow(kDeltaTs[1] / kDeltaTs[0], 2);
const idScalar expected_angular_acceleration_error_1 =
max_angular_acceleration_error[0] * pow(kDeltaTs[1] / kDeltaTs[0], 2);
printf("linear vel error: %e %e %e\n", max_linear_velocity_error[1],
expected_linear_velocity_error_1,
max_linear_velocity_error[1] - expected_linear_velocity_error_1);
printf("angular vel error: %e %e %e\n", max_angular_velocity_error[1],
expected_angular_velocity_error_1,
max_angular_velocity_error[1] - expected_angular_velocity_error_1);
printf("linear acc error: %e %e %e\n", max_linear_acceleration_error[1],
expected_linear_acceleration_error_1,
max_linear_acceleration_error[1] - expected_linear_acceleration_error_1);
printf("angular acc error: %e %e %e\n", max_angular_acceleration_error[1],
expected_angular_acceleration_error_1,
max_angular_acceleration_error[1] - expected_angular_acceleration_error_1);
*/
}
int main(int argc, char** argv)
{
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
return EXIT_SUCCESS;
}