Add premake support to build pybullet, Windows and Linux tested, will enable Mac in next commit.
Expose inverse dynamics to Bullet shared memory API, through b3CalculateInverseDynamicsCommandInit and b3GetStatusInverseDynamicsJointForces command/status. See PhysicsClientExeample or pybullet for usage. Add option for Windows and Linux to set python_lib_dir and python_include_dir for premake and --enable_pybullet option Expose inverse dynamics to pybullet: [force] = p.calculateInverseDynamics(objectIndex,[q],[qdot],[acc]) Thanks to Jeff Bingham for the suggestion.
This commit is contained in:
@@ -85,12 +85,36 @@
|
|||||||
|
|
||||||
newoption
|
newoption
|
||||||
{
|
{
|
||||||
trigger = "python",
|
trigger = "enable_pybullet",
|
||||||
description = "Enable Python scripting (experimental, use Physics Server in Example Browser). "
|
description = "Enable high-level Python scripting of Bullet with URDF/SDF import and synthetic camera."
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if os.is("Linux") then
|
||||||
|
default_python_include_dir = "/usr/include/python2.7"
|
||||||
|
default_python_lib_dir = "/usr/local/lib/"
|
||||||
|
end
|
||||||
|
|
||||||
|
|
||||||
|
if os.is("Windows") then
|
||||||
|
default_python_include_dir = "C:/Python34/include"
|
||||||
|
default_python_lib_dir = "C:/Python34/libs"
|
||||||
|
end
|
||||||
|
|
||||||
|
newoption
|
||||||
|
{
|
||||||
|
trigger = "python_include_dir",
|
||||||
|
value = default_python_include_dir,
|
||||||
|
description = "Python (2.x or 3.x) include directory"
|
||||||
|
}
|
||||||
|
|
||||||
|
newoption
|
||||||
|
{
|
||||||
|
trigger = "python_lib_dir",
|
||||||
|
value = default_python_lib_dir,
|
||||||
|
description = "Python (2.x or 3.x) library directory "
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
newoption {
|
newoption {
|
||||||
trigger = "targetdir",
|
trigger = "targetdir",
|
||||||
value = "path such as ../bin",
|
value = "path such as ../bin",
|
||||||
@@ -140,7 +164,7 @@
|
|||||||
platforms {"x32"}
|
platforms {"x32"}
|
||||||
end
|
end
|
||||||
else
|
else
|
||||||
platforms {"x32", "x64"}
|
platforms {"x64"}
|
||||||
end
|
end
|
||||||
|
|
||||||
configuration {"x32"}
|
configuration {"x32"}
|
||||||
@@ -191,6 +215,14 @@
|
|||||||
targetdir( _OPTIONS["targetdir"] or "../bin" )
|
targetdir( _OPTIONS["targetdir"] or "../bin" )
|
||||||
location("./" .. act .. postfix)
|
location("./" .. act .. postfix)
|
||||||
|
|
||||||
|
if not _OPTIONS["python_include_dir"] then
|
||||||
|
_OPTIONS["python_include_dir"] = default_python_include_dir
|
||||||
|
end
|
||||||
|
|
||||||
|
if not _OPTIONS["python_lib_dir"] then
|
||||||
|
_OPTIONS["python_lib_dir"] = default_python_lib_dir
|
||||||
|
end
|
||||||
|
|
||||||
|
|
||||||
projectRootDir = os.getcwd() .. "/../"
|
projectRootDir = os.getcwd() .. "/../"
|
||||||
print("Project root directory: " .. projectRootDir);
|
print("Project root directory: " .. projectRootDir);
|
||||||
@@ -222,7 +254,7 @@
|
|||||||
if _OPTIONS["lua"] then
|
if _OPTIONS["lua"] then
|
||||||
include "../examples/ThirdPartyLibs/lua-5.2.3"
|
include "../examples/ThirdPartyLibs/lua-5.2.3"
|
||||||
end
|
end
|
||||||
if _OPTIONS["python"] then
|
if _OPTIONS["enable_pybullet"] then
|
||||||
include "../examples/pybullet"
|
include "../examples/pybullet"
|
||||||
end
|
end
|
||||||
|
|
||||||
|
|||||||
@@ -626,6 +626,7 @@ int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle)
|
|||||||
return bodyId;
|
return bodyId;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
|
int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
|
||||||
int* bodyUniqueId,
|
int* bodyUniqueId,
|
||||||
int* numDegreeOfFreedomQ,
|
int* numDegreeOfFreedomQ,
|
||||||
@@ -1097,3 +1098,60 @@ void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUn
|
|||||||
command->m_externalForceArguments.m_numForcesAndTorques++;
|
command->m_externalForceArguments.m_numForcesAndTorques++;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
|
||||||
|
b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
|
||||||
|
const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations)
|
||||||
|
{
|
||||||
|
PhysicsClient* cl = (PhysicsClient*)physClient;
|
||||||
|
b3Assert(cl);
|
||||||
|
b3Assert(cl->canSubmitCommand());
|
||||||
|
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
|
||||||
|
b3Assert(command);
|
||||||
|
|
||||||
|
command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS;
|
||||||
|
command->m_updateFlags = 0;
|
||||||
|
command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyIndex;
|
||||||
|
int numJoints = cl->getNumJoints(bodyIndex);
|
||||||
|
for (int i = 0; i < numJoints;i++)
|
||||||
|
{
|
||||||
|
command->m_calculateInverseDynamicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
|
||||||
|
command->m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i] = jointVelocitiesQdot[i];
|
||||||
|
command->m_calculateInverseDynamicsArguments.m_jointAccelerations[i] = jointAccelerations[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
return (b3SharedMemoryCommandHandle)command;
|
||||||
|
}
|
||||||
|
|
||||||
|
int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
|
||||||
|
int* bodyUniqueId,
|
||||||
|
int* dofCount,
|
||||||
|
double* jointForces)
|
||||||
|
{
|
||||||
|
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
|
||||||
|
btAssert(status->m_type == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED);
|
||||||
|
if (status->m_type != CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED)
|
||||||
|
return false;
|
||||||
|
|
||||||
|
|
||||||
|
if (dofCount)
|
||||||
|
{
|
||||||
|
*dofCount = status->m_inverseDynamicsResultArgs.m_dofCount;
|
||||||
|
}
|
||||||
|
if (bodyUniqueId)
|
||||||
|
{
|
||||||
|
*bodyUniqueId = status->m_inverseDynamicsResultArgs.m_bodyUniqueId;
|
||||||
|
}
|
||||||
|
if (jointForces)
|
||||||
|
{
|
||||||
|
for (int i = 0; i < status->m_inverseDynamicsResultArgs.m_dofCount; i++)
|
||||||
|
{
|
||||||
|
jointForces[i] = status->m_inverseDynamicsResultArgs.m_jointForces[i];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
@@ -97,6 +97,16 @@ int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHand
|
|||||||
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
|
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
|
||||||
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
|
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
|
||||||
|
|
||||||
|
///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
|
||||||
|
b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
|
||||||
|
const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
|
||||||
|
|
||||||
|
int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
|
||||||
|
int* bodyUniqueId,
|
||||||
|
int* dofCount,
|
||||||
|
double* jointForces);
|
||||||
|
|
||||||
|
|
||||||
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
|
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
|
||||||
|
|
||||||
///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead
|
///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead
|
||||||
|
|||||||
@@ -413,6 +413,33 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
|
|||||||
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
|
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
case CMD_CALCULATE_INVERSE_DYNAMICS:
|
||||||
|
{
|
||||||
|
if (m_selectedBody >= 0)
|
||||||
|
{
|
||||||
|
btAlignedObjectArray<double> jointPositionsQ;
|
||||||
|
btAlignedObjectArray<double> jointVelocitiesQdot;
|
||||||
|
btAlignedObjectArray<double> jointAccelerations;
|
||||||
|
int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody);
|
||||||
|
if (numJoints)
|
||||||
|
{
|
||||||
|
b3Printf("Compute inverse dynamics for joint accelerations:");
|
||||||
|
jointPositionsQ.resize(numJoints);
|
||||||
|
jointVelocitiesQdot.resize(numJoints);
|
||||||
|
jointAccelerations.resize(numJoints);
|
||||||
|
for (int i = 0; i < numJoints; i++)
|
||||||
|
{
|
||||||
|
jointAccelerations[i] = 100;
|
||||||
|
b3Printf("Desired joint acceleration[%d]=%f", i, jointAccelerations[i]);
|
||||||
|
}
|
||||||
|
b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(m_physicsClientHandle,
|
||||||
|
m_selectedBody, &jointPositionsQ[0], &jointVelocitiesQdot[0], &jointAccelerations[0]);
|
||||||
|
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
default:
|
default:
|
||||||
{
|
{
|
||||||
b3Error("Unknown buttonId");
|
b3Error("Unknown buttonId");
|
||||||
@@ -490,6 +517,7 @@ void PhysicsClientExample::createButtons()
|
|||||||
createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger);
|
createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger);
|
||||||
createButton("Initialize Pose",CMD_INIT_POSE, isTrigger);
|
createButton("Initialize Pose",CMD_INIT_POSE, isTrigger);
|
||||||
createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger);
|
createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger);
|
||||||
|
createButton("Compute Inverse Dynamics", CMD_CALCULATE_INVERSE_DYNAMICS, isTrigger);
|
||||||
|
|
||||||
if (m_bodyUniqueIds.size())
|
if (m_bodyUniqueIds.size())
|
||||||
{
|
{
|
||||||
@@ -695,6 +723,36 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
|
|||||||
|
|
||||||
// b3Printf(msg);
|
// b3Printf(msg);
|
||||||
}
|
}
|
||||||
|
if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED)
|
||||||
|
{
|
||||||
|
int bodyUniqueId;
|
||||||
|
int dofCount;
|
||||||
|
|
||||||
|
b3GetStatusInverseDynamicsJointForces(status,
|
||||||
|
&bodyUniqueId,
|
||||||
|
&dofCount,
|
||||||
|
0);
|
||||||
|
|
||||||
|
btAlignedObjectArray<double> jointForces;
|
||||||
|
if (dofCount)
|
||||||
|
{
|
||||||
|
jointForces.resize(dofCount);
|
||||||
|
b3GetStatusInverseDynamicsJointForces(status,
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
&jointForces[0]);
|
||||||
|
for (int i = 0; i < dofCount; i++)
|
||||||
|
{
|
||||||
|
b3Printf("jointForces[%d]=%f", i, jointForces[i]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_FAILED)
|
||||||
|
{
|
||||||
|
b3Warning("Inverse Dynamics computations failed");
|
||||||
|
}
|
||||||
|
|
||||||
if (statusType == CMD_CAMERA_IMAGE_FAILED)
|
if (statusType == CMD_CAMERA_IMAGE_FAILED)
|
||||||
{
|
{
|
||||||
b3Warning("Camera image FAILED\n");
|
b3Warning("Camera image FAILED\n");
|
||||||
|
|||||||
@@ -542,7 +542,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
|
|||||||
b3Warning("Camera image FAILED\n");
|
b3Warning("Camera image FAILED\n");
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
case CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED:
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case CMD_CALCULATED_INVERSE_DYNAMICS_FAILED:
|
||||||
|
{
|
||||||
|
b3Warning("Inverse Dynamics computations failed");
|
||||||
|
break;
|
||||||
|
}
|
||||||
default: {
|
default: {
|
||||||
b3Error("Unknown server status\n");
|
b3Error("Unknown server status\n");
|
||||||
btAssert(0);
|
btAssert(0);
|
||||||
|
|||||||
@@ -4,13 +4,15 @@
|
|||||||
#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
|
#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
|
||||||
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
|
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
|
||||||
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
|
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
|
||||||
|
#include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp"
|
||||||
#include "TinyRendererVisualShapeConverter.h"
|
#include "TinyRendererVisualShapeConverter.h"
|
||||||
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
|
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
|
||||||
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
|
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
|
||||||
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
|
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
|
||||||
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
|
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
|
||||||
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
|
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
|
||||||
|
#include "LinearMath/btHashMap.h"
|
||||||
|
#include "BulletInverseDynamics/MultiBodyTree.hpp"
|
||||||
|
|
||||||
#include "btBulletDynamicsCommon.h"
|
#include "btBulletDynamicsCommon.h"
|
||||||
|
|
||||||
@@ -383,6 +385,7 @@ struct PhysicsServerCommandProcessorInternalData
|
|||||||
|
|
||||||
btScalar m_physicsDeltaTime;
|
btScalar m_physicsDeltaTime;
|
||||||
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
|
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
|
||||||
|
btHashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@@ -548,8 +551,25 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
|
|||||||
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
|
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
|
||||||
|
{
|
||||||
|
for (int i = 0; i < m_data->m_inverseDynamicsBodies.size(); i++)
|
||||||
|
{
|
||||||
|
btInverseDynamics::MultiBodyTree** treePtrPtr = m_data->m_inverseDynamicsBodies.getAtIndex(i);
|
||||||
|
if (treePtrPtr)
|
||||||
|
{
|
||||||
|
btInverseDynamics::MultiBodyTree* tree = *treePtrPtr;
|
||||||
|
delete tree;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
m_data->m_inverseDynamicsBodies.clear();
|
||||||
|
}
|
||||||
|
|
||||||
void PhysicsServerCommandProcessor::deleteDynamicsWorld()
|
void PhysicsServerCommandProcessor::deleteDynamicsWorld()
|
||||||
{
|
{
|
||||||
|
deleteCachedInverseDynamicsBodies();
|
||||||
|
|
||||||
|
|
||||||
for (int i=0;i<m_data->m_multiBodyJointFeedbacks.size();i++)
|
for (int i=0;i<m_data->m_multiBodyJointFeedbacks.size();i++)
|
||||||
{
|
{
|
||||||
@@ -1869,9 +1889,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
|||||||
case CMD_RESET_SIMULATION:
|
case CMD_RESET_SIMULATION:
|
||||||
{
|
{
|
||||||
//clean up all data
|
//clean up all data
|
||||||
|
deleteCachedInverseDynamicsBodies();
|
||||||
|
|
||||||
|
|
||||||
if (m_data && m_data->m_guiHelper)
|
if (m_data && m_data->m_guiHelper)
|
||||||
{
|
{
|
||||||
m_data->m_guiHelper->removeAllGraphicsInstances();
|
m_data->m_guiHelper->removeAllGraphicsInstances();
|
||||||
@@ -2066,6 +2085,74 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
|||||||
hasStatus = true;
|
hasStatus = true;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
case CMD_CALCULATE_INVERSE_DYNAMICS:
|
||||||
|
{
|
||||||
|
SharedMemoryStatus& serverCmd = serverStatusOut;
|
||||||
|
InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId);
|
||||||
|
if (bodyHandle && bodyHandle->m_multiBody)
|
||||||
|
{
|
||||||
|
btInverseDynamics::MultiBodyTree** treePtrPtr =
|
||||||
|
m_data->m_inverseDynamicsBodies.find(bodyHandle->m_multiBody);
|
||||||
|
btInverseDynamics::MultiBodyTree* tree = 0;
|
||||||
|
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
|
||||||
|
|
||||||
|
if (treePtrPtr)
|
||||||
|
{
|
||||||
|
tree = *treePtrPtr;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
btInverseDynamics::btMultiBodyTreeCreator id_creator;
|
||||||
|
if (-1 == id_creator.createFromBtMultiBody(bodyHandle->m_multiBody, false))
|
||||||
|
{
|
||||||
|
b3Error("error creating tree\n");
|
||||||
|
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
tree = btInverseDynamics::CreateMultiBodyTree(id_creator);
|
||||||
|
m_data->m_inverseDynamicsBodies.insert(bodyHandle->m_multiBody, tree);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (tree)
|
||||||
|
{
|
||||||
|
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
|
||||||
|
const int num_dofs = bodyHandle->m_multiBody->getNumDofs();
|
||||||
|
btInverseDynamics::vecx nu(num_dofs+baseDofs), qdot(num_dofs + baseDofs), q(num_dofs + baseDofs), joint_force(num_dofs + baseDofs);
|
||||||
|
for (int i = 0; i < num_dofs; i++)
|
||||||
|
{
|
||||||
|
q[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i];
|
||||||
|
qdot[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i];
|
||||||
|
nu[i+baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
if (-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
|
||||||
|
{
|
||||||
|
serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
|
||||||
|
serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs;
|
||||||
|
for (int i = 0; i < num_dofs; i++)
|
||||||
|
{
|
||||||
|
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = joint_force[i+baseDofs];
|
||||||
|
}
|
||||||
|
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
|
||||||
|
}
|
||||||
|
|
||||||
|
hasStatus = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
case CMD_APPLY_EXTERNAL_FORCE:
|
case CMD_APPLY_EXTERNAL_FORCE:
|
||||||
{
|
{
|
||||||
if (m_data->m_verboseOutput)
|
if (m_data->m_verboseOutput)
|
||||||
|
|||||||
@@ -29,6 +29,7 @@ protected:
|
|||||||
bool supportsJointMotor(class btMultiBody* body, int linkIndex);
|
bool supportsJointMotor(class btMultiBody* body, int linkIndex);
|
||||||
|
|
||||||
int createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes);
|
int createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes);
|
||||||
|
void deleteCachedInverseDynamicsBodies();
|
||||||
|
|
||||||
public:
|
public:
|
||||||
PhysicsServerCommandProcessor();
|
PhysicsServerCommandProcessor();
|
||||||
@@ -38,6 +39,7 @@ public:
|
|||||||
|
|
||||||
virtual void createEmptyDynamicsWorld();
|
virtual void createEmptyDynamicsWorld();
|
||||||
virtual void deleteDynamicsWorld();
|
virtual void deleteDynamicsWorld();
|
||||||
|
|
||||||
|
|
||||||
virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes );
|
virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes );
|
||||||
|
|
||||||
|
|||||||
@@ -348,6 +348,23 @@ enum EnumSdfRequestInfoFlags
|
|||||||
//SDF_REQUEST_INFO_CAMERA=2,
|
//SDF_REQUEST_INFO_CAMERA=2,
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
struct CalculateInverseDynamicsArgs
|
||||||
|
{
|
||||||
|
int m_bodyUniqueId;
|
||||||
|
|
||||||
|
double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
|
||||||
|
double m_jointVelocitiesQdot[MAX_DEGREE_OF_FREEDOM];
|
||||||
|
double m_jointAccelerations[MAX_DEGREE_OF_FREEDOM];
|
||||||
|
};
|
||||||
|
|
||||||
|
struct CalculateInverseDynamicsResultArgs
|
||||||
|
{
|
||||||
|
int m_bodyUniqueId;
|
||||||
|
int m_dofCount;
|
||||||
|
double m_jointForces[MAX_DEGREE_OF_FREEDOM];
|
||||||
|
};
|
||||||
|
|
||||||
struct SharedMemoryCommand
|
struct SharedMemoryCommand
|
||||||
{
|
{
|
||||||
int m_type;
|
int m_type;
|
||||||
@@ -374,6 +391,7 @@ struct SharedMemoryCommand
|
|||||||
struct RequestPixelDataArgs m_requestPixelDataArguments;
|
struct RequestPixelDataArgs m_requestPixelDataArguments;
|
||||||
struct PickBodyArgs m_pickBodyArguments;
|
struct PickBodyArgs m_pickBodyArguments;
|
||||||
struct ExternalForceArgs m_externalForceArguments;
|
struct ExternalForceArgs m_externalForceArguments;
|
||||||
|
struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments;
|
||||||
};
|
};
|
||||||
};
|
};
|
||||||
|
|
||||||
@@ -397,6 +415,7 @@ struct SharedMemoryStatus
|
|||||||
struct SendDebugLinesArgs m_sendDebugLinesArgs;
|
struct SendDebugLinesArgs m_sendDebugLinesArgs;
|
||||||
struct SendPixelDataArgs m_sendPixelDataArguments;
|
struct SendPixelDataArgs m_sendPixelDataArguments;
|
||||||
struct RigidBodyCreateArgs m_rigidBodyCreateArgs;
|
struct RigidBodyCreateArgs m_rigidBodyCreateArgs;
|
||||||
|
struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs;
|
||||||
};
|
};
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@@ -27,6 +27,7 @@ enum EnumSharedMemoryClientCommand
|
|||||||
CMD_REMOVE_PICKING_CONSTRAINT_BODY,
|
CMD_REMOVE_PICKING_CONSTRAINT_BODY,
|
||||||
CMD_REQUEST_CAMERA_IMAGE_DATA,
|
CMD_REQUEST_CAMERA_IMAGE_DATA,
|
||||||
CMD_APPLY_EXTERNAL_FORCE,
|
CMD_APPLY_EXTERNAL_FORCE,
|
||||||
|
CMD_CALCULATE_INVERSE_DYNAMICS,
|
||||||
CMD_MAX_CLIENT_COMMANDS
|
CMD_MAX_CLIENT_COMMANDS
|
||||||
};
|
};
|
||||||
|
|
||||||
@@ -59,6 +60,8 @@ enum EnumSharedMemoryServerStatus
|
|||||||
CMD_BODY_INFO_COMPLETED,
|
CMD_BODY_INFO_COMPLETED,
|
||||||
CMD_BODY_INFO_FAILED,
|
CMD_BODY_INFO_FAILED,
|
||||||
CMD_INVALID_STATUS,
|
CMD_INVALID_STATUS,
|
||||||
|
CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED,
|
||||||
|
CMD_CALCULATED_INVERSE_DYNAMICS_FAILED,
|
||||||
CMD_MAX_SERVER_COMMANDS
|
CMD_MAX_SERVER_COMMANDS
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@@ -10,7 +10,7 @@ end
|
|||||||
includedirs {".","../../src", "../ThirdPartyLibs",}
|
includedirs {".","../../src", "../ThirdPartyLibs",}
|
||||||
|
|
||||||
links {
|
links {
|
||||||
"Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath"
|
"Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath"
|
||||||
}
|
}
|
||||||
|
|
||||||
language "C++"
|
language "C++"
|
||||||
@@ -137,7 +137,7 @@ defines {"B3_USE_STANDALONE_EXAMPLE"}
|
|||||||
includedirs {"../../src"}
|
includedirs {"../../src"}
|
||||||
|
|
||||||
links {
|
links {
|
||||||
"BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common"
|
"BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common"
|
||||||
}
|
}
|
||||||
initOpenGL()
|
initOpenGL()
|
||||||
initGlew()
|
initGlew()
|
||||||
@@ -211,7 +211,7 @@ if os.is("Windows") then
|
|||||||
}
|
}
|
||||||
|
|
||||||
links {
|
links {
|
||||||
"Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath","OpenGL_Window","openvr_api"
|
"BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath","OpenGL_Window","openvr_api"
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -8,61 +8,60 @@ INCLUDE_DIRECTORIES(
|
|||||||
|
|
||||||
SET(pybullet_SRCS
|
SET(pybullet_SRCS
|
||||||
pybullet.c
|
pybullet.c
|
||||||
../../examples/ExampleBrowser/InProcessExampleBrowser.cpp
|
../../examples/ExampleBrowser/InProcessExampleBrowser.cpp
|
||||||
../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
|
../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
|
||||||
../../examples/SharedMemory/TinyRendererVisualShapeConverter.h
|
../../examples/SharedMemory/TinyRendererVisualShapeConverter.h
|
||||||
../../examples/OpenGLWindow/SimpleCamera.cpp
|
../../examples/OpenGLWindow/SimpleCamera.cpp
|
||||||
../../examples/OpenGLWindow/SimpleCamera.h
|
../../examples/OpenGLWindow/SimpleCamera.h
|
||||||
../../examples/TinyRenderer/geometry.cpp
|
../../examples/TinyRenderer/geometry.cpp
|
||||||
../../examples/TinyRenderer/model.cpp
|
../../examples/TinyRenderer/model.cpp
|
||||||
../../examples/TinyRenderer/tgaimage.cpp
|
../../examples/TinyRenderer/tgaimage.cpp
|
||||||
../../examples/TinyRenderer/our_gl.cpp
|
../../examples/TinyRenderer/our_gl.cpp
|
||||||
../../examples/TinyRenderer/TinyRenderer.cpp
|
../../examples/TinyRenderer/TinyRenderer.cpp
|
||||||
../../examples/SharedMemory/InProcessMemory.cpp
|
../../examples/SharedMemory/InProcessMemory.cpp
|
||||||
../../examples/SharedMemory/PhysicsClient.cpp
|
../../examples/SharedMemory/PhysicsClient.cpp
|
||||||
../../examples/SharedMemory/PhysicsClient.h
|
../../examples/SharedMemory/PhysicsClient.h
|
||||||
../../examples/SharedMemory/PhysicsServer.cpp
|
../../examples/SharedMemory/PhysicsServer.cpp
|
||||||
../../examples/SharedMemory/PhysicsServer.h
|
../../examples/SharedMemory/PhysicsServer.h
|
||||||
../../examples/SharedMemory/PhysicsServerExample.cpp
|
../../examples/SharedMemory/PhysicsServerExample.cpp
|
||||||
../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
|
../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
|
||||||
../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
|
../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
|
||||||
../../examples/SharedMemory/PhysicsServerSharedMemory.h
|
../../examples/SharedMemory/PhysicsServerSharedMemory.h
|
||||||
../../examples/SharedMemory/PhysicsDirect.cpp
|
../../examples/SharedMemory/PhysicsDirect.cpp
|
||||||
../../examples/SharedMemory/PhysicsDirect.h
|
../../examples/SharedMemory/PhysicsDirect.h
|
||||||
../../examples/SharedMemory/PhysicsDirectC_API.cpp
|
../../examples/SharedMemory/PhysicsDirectC_API.cpp
|
||||||
../../examples/SharedMemory/PhysicsDirectC_API.h
|
../../examples/SharedMemory/PhysicsDirectC_API.h
|
||||||
../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
|
../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
|
||||||
../../examples/SharedMemory/PhysicsServerCommandProcessor.h
|
../../examples/SharedMemory/PhysicsServerCommandProcessor.h
|
||||||
../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
|
../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
|
||||||
../../examples/SharedMemory/PhysicsClientSharedMemory.h
|
../../examples/SharedMemory/PhysicsClientSharedMemory.h
|
||||||
../../examples/SharedMemory/PhysicsClientC_API.cpp
|
../../examples/SharedMemory/PhysicsClientC_API.cpp
|
||||||
../../examples/SharedMemory/PhysicsClientC_API.h
|
../../examples/SharedMemory/PhysicsClientC_API.h
|
||||||
../../examples/SharedMemory/Win32SharedMemory.cpp
|
../../examples/SharedMemory/Win32SharedMemory.cpp
|
||||||
../../examples/SharedMemory/Win32SharedMemory.h
|
../../examples/SharedMemory/Win32SharedMemory.h
|
||||||
../../examples/SharedMemory/PosixSharedMemory.cpp
|
../../examples/SharedMemory/PosixSharedMemory.cpp
|
||||||
../../examples/SharedMemory/PosixSharedMemory.h
|
../../examples/SharedMemory/PosixSharedMemory.h
|
||||||
../../examples/Utils/b3ResourcePath.cpp
|
../../examples/Utils/b3ResourcePath.cpp
|
||||||
../../examples/Utils/b3ResourcePath.h
|
../../examples/Utils/b3ResourcePath.h
|
||||||
../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp
|
../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp
|
||||||
../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp
|
../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp
|
||||||
../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp
|
../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp
|
||||||
../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp
|
../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp
|
||||||
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
|
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
|
||||||
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
|
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
|
||||||
../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
|
../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
|
||||||
../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
|
../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
|
||||||
../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
|
../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
|
||||||
../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
|
../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
|
||||||
../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
|
../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
|
||||||
../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
|
../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
|
||||||
../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
|
../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
|
||||||
../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
|
../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
|
||||||
../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
|
../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
|
||||||
../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
|
../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
|
||||||
../../examples/MultiThreading/b3PosixThreadSupport.cpp
|
../../examples/MultiThreading/b3PosixThreadSupport.cpp
|
||||||
../../examples/MultiThreading/b3Win32ThreadSupport.cpp
|
../../examples/MultiThreading/b3Win32ThreadSupport.cpp
|
||||||
../../examples/MultiThreading/b3ThreadSupportInterface.cpp
|
../../examples/MultiThreading/b3ThreadSupportInterface.cpp
|
||||||
|
|
||||||
)
|
)
|
||||||
|
|
||||||
IF(WIN32)
|
IF(WIN32)
|
||||||
|
|||||||
@@ -1,7 +1,6 @@
|
|||||||
|
|
||||||
|
|
||||||
project ("pybullet")
|
project ("pybullet")
|
||||||
|
|
||||||
language "C++"
|
language "C++"
|
||||||
kind "SharedLib"
|
kind "SharedLib"
|
||||||
targetsuffix ("")
|
targetsuffix ("")
|
||||||
@@ -12,7 +11,7 @@ project ("pybullet")
|
|||||||
defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
|
defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
|
||||||
hasCL = findOpenCL("clew")
|
hasCL = findOpenCL("clew")
|
||||||
|
|
||||||
links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"}
|
links{"BulletExampleBrowserLib","gwen", "BulletFileLoader","BulletWorldImporter","OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"}
|
||||||
initOpenGL()
|
initOpenGL()
|
||||||
initGlew()
|
initGlew()
|
||||||
|
|
||||||
@@ -20,10 +19,8 @@ project ("pybullet")
|
|||||||
".",
|
".",
|
||||||
"../../src",
|
"../../src",
|
||||||
"../ThirdPartyLibs",
|
"../ThirdPartyLibs",
|
||||||
"/usr/include/python2.7",
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
if os.is("MacOSX") then
|
if os.is("MacOSX") then
|
||||||
links{"Cocoa.framework","Python"}
|
links{"Cocoa.framework","Python"}
|
||||||
end
|
end
|
||||||
@@ -40,8 +37,69 @@ project ("pybullet")
|
|||||||
|
|
||||||
files {
|
files {
|
||||||
"pybullet.c",
|
"pybullet.c",
|
||||||
"../../examples/ExampleBrowser/ExampleEntries.cpp",
|
"../../examples/ExampleBrowser/InProcessExampleBrowser.cpp",
|
||||||
|
"../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp",
|
||||||
|
"../../examples/SharedMemory/TinyRendererVisualShapeConverter.h",
|
||||||
|
"../../examples/OpenGLWindow/SimpleCamera.cpp",
|
||||||
|
"../../examples/OpenGLWindow/SimpleCamera.h",
|
||||||
|
"../../examples/TinyRenderer/geometry.cpp",
|
||||||
|
"../../examples/TinyRenderer/model.cpp",
|
||||||
|
"../../examples/TinyRenderer/tgaimage.cpp",
|
||||||
|
"../../examples/TinyRenderer/our_gl.cpp",
|
||||||
|
"../../examples/TinyRenderer/TinyRenderer.cpp",
|
||||||
|
"../../examples/SharedMemory/InProcessMemory.cpp",
|
||||||
|
"../../examples/SharedMemory/PhysicsClient.cpp",
|
||||||
|
"../../examples/SharedMemory/PhysicsClient.h",
|
||||||
|
"../../examples/SharedMemory/PhysicsServer.cpp",
|
||||||
|
"../../examples/SharedMemory/PhysicsServer.h",
|
||||||
|
"../../examples/SharedMemory/PhysicsServerExample.cpp",
|
||||||
|
"../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp",
|
||||||
|
"../../examples/SharedMemory/PhysicsServerSharedMemory.cpp",
|
||||||
|
"../../examples/SharedMemory/PhysicsServerSharedMemory.h",
|
||||||
|
"../../examples/SharedMemory/PhysicsDirect.cpp",
|
||||||
|
"../../examples/SharedMemory/PhysicsDirect.h",
|
||||||
|
"../../examples/SharedMemory/PhysicsDirectC_API.cpp",
|
||||||
|
"../../examples/SharedMemory/PhysicsDirectC_API.h",
|
||||||
|
"../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp",
|
||||||
|
"../../examples/SharedMemory/PhysicsServerCommandProcessor.h",
|
||||||
|
"../../examples/SharedMemory/PhysicsClientSharedMemory.cpp",
|
||||||
|
"../../examples/SharedMemory/PhysicsClientSharedMemory.h",
|
||||||
|
"../../examples/SharedMemory/PhysicsClientC_API.cpp",
|
||||||
|
"../../examples/SharedMemory/PhysicsClientC_API.h",
|
||||||
|
"../../examples/SharedMemory/Win32SharedMemory.cpp",
|
||||||
|
"../../examples/SharedMemory/Win32SharedMemory.h",
|
||||||
|
"../../examples/SharedMemory/PosixSharedMemory.cpp",
|
||||||
|
"../../examples/SharedMemory/PosixSharedMemory.h",
|
||||||
|
"../../examples/Utils/b3ResourcePath.cpp",
|
||||||
|
"../../examples/Utils/b3ResourcePath.h",
|
||||||
|
"../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp",
|
||||||
|
"../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp",
|
||||||
|
"../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
|
||||||
|
"../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
|
||||||
|
"../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
|
||||||
|
"../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
|
||||||
|
"../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",
|
||||||
|
"../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
|
||||||
|
"../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp",
|
||||||
|
"../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
|
||||||
|
"../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
|
||||||
|
"../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
|
||||||
|
"../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp",
|
||||||
|
"../../examples/Importers/ImportURDFDemo/UrdfParser.cpp",
|
||||||
|
"../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp",
|
||||||
|
"../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
|
||||||
|
"../../examples/MultiThreading/b3PosixThreadSupport.cpp",
|
||||||
|
"../../examples/MultiThreading/b3Win32ThreadSupport.cpp",
|
||||||
|
"../../examples/MultiThreading/b3ThreadSupportInterface.cpp",
|
||||||
}
|
}
|
||||||
|
|
||||||
|
includedirs {
|
||||||
|
_OPTIONS["python_include_dir"],
|
||||||
|
}
|
||||||
|
libdirs {
|
||||||
|
_OPTIONS["python_lib_dir"]
|
||||||
|
}
|
||||||
|
|
||||||
if os.is("Linux") then
|
if os.is("Linux") then
|
||||||
initX11()
|
initX11()
|
||||||
end
|
end
|
||||||
|
|||||||
@@ -1163,7 +1163,7 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
|
|||||||
if (PyArg_ParseTuple(args, "iiOffffifff", &width, &height, &objTargetPos, &camDistance, &yaw, &pitch, &roll, &upAxisIndex, &nearVal, &farVal, &fov))
|
if (PyArg_ParseTuple(args, "iiOffffifff", &width, &height, &objTargetPos, &camDistance, &yaw, &pitch, &roll, &upAxisIndex, &nearVal, &farVal, &fov))
|
||||||
{
|
{
|
||||||
|
|
||||||
b3RequestCameraImageSetPixelResolution(command,width,height);
|
b3RequestCameraImageSetPixelResolution(command,width,height);
|
||||||
if (pybullet_internalSetVector(objTargetPos, targetPos))
|
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);
|
//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);
|
||||||
@@ -1521,6 +1521,123 @@ static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, PyObject* args)
|
|||||||
return 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<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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
static PyMethodDef SpamMethods[] = {
|
static PyMethodDef SpamMethods[] = {
|
||||||
|
|
||||||
@@ -1587,6 +1704,8 @@ static PyMethodDef SpamMethods[] = {
|
|||||||
{"getEulerFromQuaternion", pybullet_getEulerFromQuaternion, METH_VARARGS,
|
{"getEulerFromQuaternion", pybullet_getEulerFromQuaternion, METH_VARARGS,
|
||||||
"Convert quaternion [x,y,z,w] to Euler [roll, pitch, yaw] as in URDF/SDF convention"},
|
"Convert quaternion [x,y,z,w] to Euler [roll, pitch, yaw] as in URDF/SDF convention"},
|
||||||
|
|
||||||
|
{ "calculateInverseDynamics", pybullet_calculateInverseDynamics, METH_VARARGS,
|
||||||
|
"Given an object id, joint positions, joint velocities and joint accelerations, compute the joint forces using Inverse Dynamics" },
|
||||||
//todo(erwincoumans)
|
//todo(erwincoumans)
|
||||||
//saveSnapshot
|
//saveSnapshot
|
||||||
//loadSnapshot
|
//loadSnapshot
|
||||||
|
|||||||
@@ -13,7 +13,7 @@ ADD_DEFINITIONS(-DPHYSICS_LOOP_BACK -DPHYSICS_SERVER_DIRECT -DENABLE_GTEST -D_VA
|
|||||||
|
|
||||||
|
|
||||||
LINK_LIBRARIES(
|
LINK_LIBRARIES(
|
||||||
BulletFileLoader BulletWorldImporter Bullet3Common BulletDynamics BulletCollision LinearMath gtest
|
BulletInverseDynamicsUtils BulletInverseDynamics BulletFileLoader BulletWorldImporter Bullet3Common BulletDynamics BulletCollision LinearMath gtest
|
||||||
)
|
)
|
||||||
|
|
||||||
IF (NOT WIN32)
|
IF (NOT WIN32)
|
||||||
|
|||||||
@@ -36,6 +36,8 @@ project ("Test_PhysicsServerLoopBack")
|
|||||||
"../../examples/ThirdPartyLibs"}
|
"../../examples/ThirdPartyLibs"}
|
||||||
defines {"PHYSICS_LOOP_BACK"}
|
defines {"PHYSICS_LOOP_BACK"}
|
||||||
links {
|
links {
|
||||||
|
"BulletInverseDynamicsUtils",
|
||||||
|
"BulletInverseDynamics",
|
||||||
"BulletFileLoader",
|
"BulletFileLoader",
|
||||||
"BulletWorldImporter",
|
"BulletWorldImporter",
|
||||||
"Bullet3Common",
|
"Bullet3Common",
|
||||||
@@ -104,6 +106,8 @@ project ("Test_PhysicsServerLoopBack")
|
|||||||
"../../examples/ThirdPartyLibs"}
|
"../../examples/ThirdPartyLibs"}
|
||||||
defines {"PHYSICS_SERVER_DIRECT"}
|
defines {"PHYSICS_SERVER_DIRECT"}
|
||||||
links {
|
links {
|
||||||
|
"BulletInverseDynamicsUtils",
|
||||||
|
"BulletInverseDynamics",
|
||||||
"BulletFileLoader",
|
"BulletFileLoader",
|
||||||
"BulletWorldImporter",
|
"BulletWorldImporter",
|
||||||
"Bullet3Common",
|
"Bullet3Common",
|
||||||
@@ -213,55 +217,55 @@ project ("Test_PhysicsServerInProcessExampleBrowser")
|
|||||||
"../../examples/ExampleBrowser/InProcessExampleBrowser.cpp",
|
"../../examples/ExampleBrowser/InProcessExampleBrowser.cpp",
|
||||||
"../../examples/SharedMemory/InProcessMemory.cpp",
|
"../../examples/SharedMemory/InProcessMemory.cpp",
|
||||||
"../../examples/SharedMemory/PhysicsClient.cpp",
|
"../../examples/SharedMemory/PhysicsClient.cpp",
|
||||||
"../../examples/SharedMemory/PhysicsClient.h",
|
"../../examples/SharedMemory/PhysicsClient.h",
|
||||||
"../../examples/SharedMemory/PhysicsServer.cpp",
|
"../../examples/SharedMemory/PhysicsServer.cpp",
|
||||||
"../../examples/SharedMemory/PhysicsServer.h",
|
"../../examples/SharedMemory/PhysicsServer.h",
|
||||||
"../../examples/SharedMemory/PhysicsServerExample.cpp",
|
"../../examples/SharedMemory/PhysicsServerExample.cpp",
|
||||||
"../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp",
|
"../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp",
|
||||||
"../../examples/SharedMemory/PhysicsServerSharedMemory.cpp",
|
"../../examples/SharedMemory/PhysicsServerSharedMemory.cpp",
|
||||||
"../../examples/SharedMemory/PhysicsServerSharedMemory.h",
|
"../../examples/SharedMemory/PhysicsServerSharedMemory.h",
|
||||||
"../../examples/SharedMemory/PhysicsDirect.cpp",
|
"../../examples/SharedMemory/PhysicsDirect.cpp",
|
||||||
"../../examples/SharedMemory/PhysicsDirect.h",
|
"../../examples/SharedMemory/PhysicsDirect.h",
|
||||||
"../../examples/SharedMemory/PhysicsDirectC_API.cpp",
|
"../../examples/SharedMemory/PhysicsDirectC_API.cpp",
|
||||||
"../../examples/SharedMemory/PhysicsDirectC_API.h",
|
"../../examples/SharedMemory/PhysicsDirectC_API.h",
|
||||||
"../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp",
|
"../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp",
|
||||||
"../../examples/SharedMemory/PhysicsServerCommandProcessor.h",
|
"../../examples/SharedMemory/PhysicsServerCommandProcessor.h",
|
||||||
"../../examples/SharedMemory/PhysicsClientSharedMemory.cpp",
|
"../../examples/SharedMemory/PhysicsClientSharedMemory.cpp",
|
||||||
"../../examples/SharedMemory/PhysicsClientSharedMemory.h",
|
"../../examples/SharedMemory/PhysicsClientSharedMemory.h",
|
||||||
"../../examples/SharedMemory/PhysicsClientC_API.cpp",
|
"../../examples/SharedMemory/PhysicsClientC_API.cpp",
|
||||||
"../../examples/SharedMemory/PhysicsClientC_API.h",
|
"../../examples/SharedMemory/PhysicsClientC_API.h",
|
||||||
"../../examples/SharedMemory/Win32SharedMemory.cpp",
|
"../../examples/SharedMemory/Win32SharedMemory.cpp",
|
||||||
"../../examples/SharedMemory/Win32SharedMemory.h",
|
"../../examples/SharedMemory/Win32SharedMemory.h",
|
||||||
"../../examples/SharedMemory/PosixSharedMemory.cpp",
|
"../../examples/SharedMemory/PosixSharedMemory.cpp",
|
||||||
"../../examples/SharedMemory/PosixSharedMemory.h",
|
"../../examples/SharedMemory/PosixSharedMemory.h",
|
||||||
"../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp",
|
"../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp",
|
||||||
"../../examples/SharedMemory/TinyRendererVisualShapeConverter.h",
|
"../../examples/SharedMemory/TinyRendererVisualShapeConverter.h",
|
||||||
"../../examples/TinyRenderer/geometry.cpp",
|
"../../examples/TinyRenderer/geometry.cpp",
|
||||||
"../../examples/TinyRenderer/model.cpp",
|
"../../examples/TinyRenderer/model.cpp",
|
||||||
"../../examples/TinyRenderer/tgaimage.cpp",
|
"../../examples/TinyRenderer/tgaimage.cpp",
|
||||||
"../../examples/TinyRenderer/our_gl.cpp",
|
"../../examples/TinyRenderer/our_gl.cpp",
|
||||||
"../../examples/TinyRenderer/TinyRenderer.cpp",
|
"../../examples/TinyRenderer/TinyRenderer.cpp",
|
||||||
"../../examples/Utils/b3ResourcePath.cpp",
|
"../../examples/Utils/b3ResourcePath.cpp",
|
||||||
"../../examples/Utils/b3ResourcePath.h",
|
"../../examples/Utils/b3ResourcePath.h",
|
||||||
"../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp",
|
"../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp",
|
||||||
"../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp",
|
"../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp",
|
||||||
"../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
|
"../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
|
||||||
"../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
|
"../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
|
||||||
"../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
|
"../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
|
||||||
"../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
|
"../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
|
||||||
"../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
|
"../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
|
||||||
"../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp",
|
"../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp",
|
||||||
"../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
|
"../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
|
||||||
"../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
|
"../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
|
||||||
"../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
|
"../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
|
||||||
"../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp",
|
"../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp",
|
||||||
"../../examples/Importers/ImportURDFDemo/UrdfParser.cpp",
|
"../../examples/Importers/ImportURDFDemo/UrdfParser.cpp",
|
||||||
"../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp",
|
"../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp",
|
||||||
"../../examples/MultiThreading/b3PosixThreadSupport.cpp",
|
"../../examples/MultiThreading/b3PosixThreadSupport.cpp",
|
||||||
"../../examples/MultiThreading/b3Win32ThreadSupport.cpp",
|
"../../examples/MultiThreading/b3Win32ThreadSupport.cpp",
|
||||||
"../../examples/MultiThreading/b3ThreadSupportInterface.cpp",
|
"../../examples/MultiThreading/b3ThreadSupportInterface.cpp",
|
||||||
"../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
|
"../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
|
||||||
"../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",
|
"../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",
|
||||||
}
|
}
|
||||||
if os.is("Linux") then
|
if os.is("Linux") then
|
||||||
initX11()
|
initX11()
|
||||||
|
|||||||
Reference in New Issue
Block a user