Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
yunfeibai
2016-09-11 04:11:51 -07:00
parent a00841a498 30bd1d7362
commit c952c50ad9
66 changed files with 2812 additions and 1923 deletions
Download Patch File Download Diff File Expand all files Collapse all files

180
examples/SharedMemory/IKTrajectoryHelper.cpp Normal file
View File

@@ -0,0 +1,180 @@
#include "IKTrajectoryHelper.h"
#include "BussIK/Node.h"
#include "BussIK/Tree.h"
#include "BussIK/Jacobian.h"
#include "BussIK/VectorRn.h"
#include "BussIK/MatrixRmn.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "BulletDynamics/Featherstone/btMultiBody.h"
#define RADIAN(X) ((X)*RadiansToDegrees)
//use BussIK and Reflexxes to convert from Cartesian endeffector future target to
//joint space positions at each real-time (simulation) step
struct IKTrajectoryHelperInternalData
{
VectorR3 m_endEffectorTargetPosition;
Tree m_ikTree;
b3AlignedObjectArray<Node*> m_ikNodes;
Jacobian* m_ikJacobian;
IKTrajectoryHelperInternalData()
{
m_endEffectorTargetPosition.SetZero();
}
};
IKTrajectoryHelper::IKTrajectoryHelper()
{
m_data = new IKTrajectoryHelperInternalData;
}
IKTrajectoryHelper::~IKTrajectoryHelper()
{
delete m_data;
}
bool IKTrajectoryHelper::createFromMultiBody(class btMultiBody* mb)
{
//todo: implement proper conversion. For now, we only 'detect' a likely KUKA iiwa and hardcode its creation
if (mb->getNumLinks()==7)
{
createKukaIIWA();
return true;
}
return false;
}
void IKTrajectoryHelper::createKukaIIWA()
{
const VectorR3& unitx = VectorR3::UnitX;
const VectorR3& unity = VectorR3::UnitY;
const VectorR3& unitz = VectorR3::UnitZ;
const VectorR3 unit1(sqrt(14.0) / 8.0, 1.0 / 8.0, 7.0 / 8.0);
const VectorR3& zero = VectorR3::Zero;
float minTheta = -4 * PI;
float maxTheta = 4 * PI;
m_data->m_ikNodes.resize(8);//7DOF+additional endeffector
m_data->m_ikNodes[0] = new Node(VectorR3(0.100000, 0.000000, 0.087500), unitz, 0.08, JOINT, -1e30, 1e30, RADIAN(0.));
m_data->m_ikTree.InsertRoot(m_data->m_ikNodes[0]);
m_data->m_ikNodes[1] = new Node(VectorR3(0.100000, -0.000000, 0.290000), unity, 0.08, JOINT, -0.5, 0.4, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[0], m_data->m_ikNodes[1]);
m_data->m_ikNodes[2] = new Node(VectorR3(0.100000, -0.000000, 0.494500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[1], m_data->m_ikNodes[2]);
m_data->m_ikNodes[3] = new Node(VectorR3(0.100000, 0.000000, 0.710000), -unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[2], m_data->m_ikNodes[3]);
m_data->m_ikNodes[4] = new Node(VectorR3(0.100000, 0.000000, 0.894500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[3], m_data->m_ikNodes[4]);
m_data->m_ikNodes[5] = new Node(VectorR3(0.100000, 0.000000, 1.110000), unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[4], m_data->m_ikNodes[5]);
m_data->m_ikNodes[6] = new Node(VectorR3(0.100000, 0.000000, 1.191000), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[5], m_data->m_ikNodes[6]);
m_data->m_ikNodes[7] = new Node(VectorR3(0.100000, 0.000000, 1.20000), zero, 0.08, EFFECTOR);
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[6], m_data->m_ikNodes[7]);
m_data->m_ikJacobian = new Jacobian(&m_data->m_ikTree);
// Reset(m_ikTree,m_ikJacobian);
m_data->m_ikTree.Init();
m_data->m_ikTree.Compute();
m_data->m_ikJacobian->Reset();
}
bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double endEffectorWorldPosition[3],
const double* q_current, int numQ,
double* q_new, int ikMethod, const double* linear_jacobian, int jacobian_size)
{
if (numQ != 7)
{
return false;
}
for (int i=0;i<numQ;i++)
{
m_data->m_ikNodes[i]->SetTheta(q_current[i]);
}
bool UseJacobianTargets1 = false;
if ( UseJacobianTargets1 ) {
m_data->m_ikJacobian->SetJtargetActive();
}
else {
m_data->m_ikJacobian->SetJendActive();
}
VectorR3 targets;
targets.Set(endEffectorTargetPosition[0],endEffectorTargetPosition[1],endEffectorTargetPosition[2]);
m_data->m_ikJacobian->ComputeJacobian(&targets); // Set up Jacobian and deltaS vectors
// Set one end effector world position from Bullet
VectorRn deltaS(3);
for (int i = 0; i < 3; ++i)
{
deltaS.Set(i,endEffectorTargetPosition[i]-endEffectorWorldPosition[i]);
}
m_data->m_ikJacobian->SetDeltaS(deltaS);
// Set Jacobian from Bullet body Jacobian
int nRow = m_data->m_ikJacobian->GetNumRows();
int nCol = m_data->m_ikJacobian->GetNumCols();
b3Assert(jacobian_size==nRow*nCol);
MatrixRmn linearJacobian(nRow,nCol);
for (int i = 0; i < nRow; ++i)
{
for (int j = 0; j < nCol; ++j)
{
linearJacobian.Set(i,j,linear_jacobian[i*nCol+j]);
}
}
m_data->m_ikJacobian->SetJendTrans(linearJacobian);
// Calculate the change in theta values
switch (ikMethod) {
case IK2_JACOB_TRANS:
m_data->m_ikJacobian->CalcDeltaThetasTranspose(); // Jacobian transpose method
break;
case IK2_DLS:
m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method
break;
case IK2_DLS_SVD:
m_data->m_ikJacobian->CalcDeltaThetasDLSwithSVD();
break;
case IK2_PURE_PSEUDO:
m_data->m_ikJacobian->CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
break;
case IK2_SDLS:
m_data->m_ikJacobian->CalcDeltaThetasSDLS(); // Selectively damped least squares method
break;
default:
m_data->m_ikJacobian->ZeroDeltaThetas();
break;
}
m_data->m_ikJacobian->UpdateThetas();
// Apply the change in the theta values
m_data->m_ikJacobian->UpdatedSClampValue(&targets);
for (int i=0;i<numQ;i++)
{
q_new[i] = m_data->m_ikNodes[i]->GetTheta();
}
return true;
}

33
examples/SharedMemory/IKTrajectoryHelper.h Normal file
View File

@@ -0,0 +1,33 @@
#ifndef IK_TRAJECTORY_HELPER_H
#define IK_TRAJECTORY_HELPER_H
enum IK2_Method
{
IK2_JACOB_TRANS=0,
IK2_PURE_PSEUDO,
IK2_DLS,
IK2_SDLS ,
IK2_DLS_SVD
};
class IKTrajectoryHelper
{
struct IKTrajectoryHelperInternalData* m_data;
public:
IKTrajectoryHelper();
virtual ~IKTrajectoryHelper();
///todo: replace createKukaIIWA with a generic way of create an IK tree
void createKukaIIWA();
bool createFromMultiBody(class btMultiBody* mb);
bool computeIK(const double endEffectorTargetPosition[3],
const double endEffectorWorldPosition[3],
const double* q_old, int numQ,
double* q_new, int ikMethod, const double* linear_jacobian, int jacobian_size);
};
#endif //IK_TRAJECTORY_HELPER_H

70
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -163,6 +163,19 @@ int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int
return 0;
}
int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_updateFlags |= SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP;
command->m_physSimParamArgs.m_defaultContactERP = defaultContactERP;
return 0;
}
int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -1303,4 +1316,61 @@ int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJ
}
return true;
}
///compute the joint positions to move the end effector to a desired target using inverse kinematics
b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
const double* jointPositionsQ, const double targetPosition[3])
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CALCULATE_INVERSE_KINEMATICS;
command->m_updateFlags = 0;
command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyIndex;
int numJoints = cl->getNumJoints(bodyIndex);
for (int i = 0; i < numJoints;i++)
{
command->m_calculateInverseKinematicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
}
command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0];
command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1];
command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2];
return (b3SharedMemoryCommandHandle)command;
}
int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,
double* jointPositions)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
btAssert(status->m_type == CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED);
if (status->m_type != CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED)
return false;
if (dofCount)
{
*dofCount = status->m_inverseKinematicsResultArgs.m_dofCount;
}
if (bodyUniqueId)
{
*bodyUniqueId = status->m_inverseKinematicsResultArgs.m_bodyUniqueId;
}
if (jointPositions)
{
for (int i = 0; i < status->m_inverseKinematicsResultArgs.m_dofCount; i++)
{
jointPositions[i] = status->m_inverseKinematicsResultArgs.m_jointPositions[i];
}
}
return false;
}

10
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -89,6 +89,7 @@ void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3Con
b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);
int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep);
int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
@@ -119,6 +120,15 @@ b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle
int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian);
///compute the joint positions to move the end effector to a desired target using inverse kinematics
b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
const double* jointPositionsQ, const double targetPosition[3]);
int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,
double* jointPositions);
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead

162
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -15,7 +15,7 @@
#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
#include "LinearMath/btHashMap.h"
#include "BulletInverseDynamics/MultiBodyTree.hpp"
#include "IKTrajectoryHelper.h"
#include "btBulletDynamicsCommon.h"
#include "LinearMath/btTransform.h"
@@ -381,6 +381,9 @@ struct PhysicsServerCommandProcessorInternalData
bool m_allowRealTimeSimulation;
bool m_hasGround;
btMultiBodyFixedConstraint* m_gripperRigidbodyFixed;
btMultiBody* m_gripperMultiBody;
CommandLogger* m_commandLogger;
CommandLogPlayback* m_logPlayback;
@@ -389,6 +392,7 @@ struct PhysicsServerCommandProcessorInternalData
btScalar m_numSimulationSubSteps;
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
btHashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
btHashMap<btHashPtr, IKTrajectoryHelper*> m_inverseKinematicsHelpers;
@@ -428,6 +432,8 @@ struct PhysicsServerCommandProcessorInternalData
PhysicsServerCommandProcessorInternalData()
:m_hasGround(false),
m_gripperRigidbodyFixed(0),
m_gripperMultiBody(0),
m_allowRealTimeSimulation(false),
m_commandLogger(0),
m_logPlayback(0),
@@ -552,10 +558,14 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
//Workaround: in a VR application, where we avoid synchronizaing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(8192);
m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer();
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.005;
}
void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
@@ -1061,6 +1071,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
//no timestamp yet
int timeStamp = 0;
//catch uninitialized cases
serverStatusOut.m_type = CMD_INVALID_STATUS;
//consume the command
switch (clientCmd.m_type)
@@ -1868,6 +1881,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps;
}
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP)
{
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = clientCmd.m_physSimParamArgs.m_defaultContactERP;
}
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
hasStatus = true;
@@ -1961,6 +1979,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED;
hasStatus = true;
m_data->m_hasGround = false;
m_data->m_gripperRigidbodyFixed = 0;
break;
}
case CMD_CREATE_RIGID_BODY:
@@ -2516,9 +2535,50 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
}
}
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
hasStatus = true;
break;
}
case CMD_CALCULATE_INVERSE_KINEMATICS:
{
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_FAILED;
InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId);
if (bodyHandle && bodyHandle->m_multiBody)
{
IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody);
IKTrajectoryHelper* ikHelperPtr = 0;
if (ikHelperPtrPtr)
{
ikHelperPtr = *ikHelperPtrPtr;
}
else
{
IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper;
if (tmpHelper->createFromMultiBody(bodyHandle->m_multiBody))
{
m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, ikHelperPtr);
ikHelperPtr = tmpHelper;
} else
{
delete tmpHelper;
}
}
if (ikHelperPtr)
{
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
}
}
hasStatus = true;
break;
}
default:
{
b3Error("Unknown command encountered");
@@ -2536,13 +2596,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
return hasStatus;
}
static int skip=1;
void PhysicsServerCommandProcessor::renderScene()
{
if (m_data->m_guiHelper)
{
m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld);
m_data->m_guiHelper->render(m_data->m_dynamicsWorld);
}
@@ -2560,6 +2620,7 @@ void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags)
}
}
btVector3 gLastPickPos(0,0,0);
bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
{
@@ -2573,6 +2634,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
{
btVector3 pickPos = rayCallback.m_hitPointWorld;
gLastPickPos = pickPos;
btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject);
if (body)
{
@@ -2591,6 +2653,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
//very weak constraint for picking
p2p->m_setting.m_tau = 0.001f;
}
} else
{
btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject);
@@ -2709,23 +2772,108 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
m_data->m_logPlayback = pb;
}
btVector3 gVRGripperPos(0,0,0);
btQuaternion gVRGripperOrn(0,0,0,1);
bool gVRGripperClosed = false;
int gDroppedSimulationSteps = 0;
int gNumSteps = 0;
double gDtInSec = 0.f;
double gSubStep = 0.f;
void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
{
if (m_data->m_allowRealTimeSimulation)
if (m_data->m_allowRealTimeSimulation && m_data->m_guiHelper)
{
static btAlignedObjectArray<char> gBufferServerToClient;
gBufferServerToClient.resize(32768);
if (!m_data->m_hasGround)
{
m_data->m_hasGround = true;
int bodyId = 0;
btAlignedObjectArray<char> bufferServerToClient;
bufferServerToClient.resize(32768);
loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
if (m_data->m_gripperRigidbodyFixed == 0)
{
int bodyId = 0;
loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &bufferServerToClient[0], bufferServerToClient.size());
if (loadUrdf("pr2_gripper.urdf", btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()))
{
InteralBodyData* parentBody = m_data->getHandle(bodyId);
if (parentBody->m_multiBody)
{
parentBody->m_multiBody->setHasSelfCollision(1);
btVector3 pivotInParent(0, 0, 0);
btMatrix3x3 frameInParent;
frameInParent.setRotation(btQuaternion(0, 0, 0, 1));
btVector3 pivotInChild(0, 0, 0);
btMatrix3x3 frameInChild;
frameInChild.setIdentity();
m_data->m_gripperRigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody, -1, 0, pivotInParent, pivotInChild, frameInParent, frameInChild);
m_data->m_gripperMultiBody = parentBody->m_multiBody;
if (m_data->m_gripperMultiBody->getNumLinks() > 2)
{
m_data->m_gripperMultiBody->setJointPos(0, SIMD_HALF_PI);
m_data->m_gripperMultiBody->setJointPos(2, SIMD_HALF_PI);
}
m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(2.);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed);
}
}
}
}
m_data->m_dynamicsWorld->stepSimulation(dtInSec,10,m_data->m_physicsDeltaTime);
if (m_data->m_gripperRigidbodyFixed && m_data->m_gripperMultiBody)
{
m_data->m_gripperRigidbodyFixed->setFrameInB(btMatrix3x3(gVRGripperOrn));
m_data->m_gripperRigidbodyFixed->setPivotInB(gVRGripperPos);
if (m_data->m_gripperMultiBody->getNumLinks() > 2)
{
for (int i = 0; i < 2; i++)
{
if (supportsJointMotor(m_data->m_gripperMultiBody, i * 2))
{
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i * 2).m_userPtr;
if (motor)
{
motor->setErp(0.005);
if (gVRGripperClosed)
{
motor->setPositionTarget(0.2, 1);
}
else
{
motor->setPositionTarget(SIMD_HALF_PI, 1);
}
motor->setVelocityTarget(0, 0.1);
btScalar maxImp = 550.*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp);
}
}
}
}
}
int maxSteps = 3;
int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec,maxSteps,m_data->m_physicsDeltaTime);
gDroppedSimulationSteps += numSteps > maxSteps ? numSteps - maxSteps : 0;
if (numSteps)
{
gNumSteps = numSteps;
gDtInSec = dtInSec;
gSubStep = m_data->m_physicsDeltaTime;
}
}
}

297
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -13,8 +13,14 @@
#include "../Utils/b3Clock.h"
#include "../MultiThreading/b3ThreadSupportInterface.h"
#define MAX_VR_CONTROLLERS 8
extern btVector3 gLastPickPos;
btVector3 gVRTeleportPos(0,0,0);
extern bool gVRGripperClosed;
bool gDebugRenderToggle = false;
void MotionThreadFunc(void* userPtr,void* lsMemory);
void* MotionlsMemoryFunc();
#define MAX_MOTION_NUM_THREADS 1
@@ -82,22 +88,27 @@ b3ThreadSupportInterface* createMotionThreadSupport(int numThreads)
struct MotionArgs
{
MotionArgs()
:m_physicsServerPtr(0),
m_isPicking(false),
m_isDragging(false),
m_isReleasing(false)
:m_physicsServerPtr(0)
{
for (int i=0;i<MAX_VR_CONTROLLERS;i++)
{
m_isVrControllerPicking[i] = false;
m_isVrControllerDragging[i] = false;
m_isVrControllerReleasing[i] = false;
m_isVrControllerTeleporting[i] = false;
}
}
b3CriticalSection* m_cs;
PhysicsServerSharedMemory* m_physicsServerPtr;
b3AlignedObjectArray<b3Vector3> m_positions;
btVector3 m_pos;
btQuaternion m_orn;
bool m_isPicking;
bool m_isDragging;
bool m_isReleasing;
btVector3 m_vrControllerPos[MAX_VR_CONTROLLERS];
btQuaternion m_vrControllerOrn[MAX_VR_CONTROLLERS];
bool m_isVrControllerPicking[MAX_VR_CONTROLLERS];
bool m_isVrControllerDragging[MAX_VR_CONTROLLERS];
bool m_isVrControllerReleasing[MAX_VR_CONTROLLERS];
bool m_isVrControllerTeleporting[MAX_VR_CONTROLLERS];
};
@@ -108,6 +119,7 @@ struct MotionThreadLocalStorage
int skip = 0;
int skip1 = 0;
float clampedDeltaTime = 0.2;
void MotionThreadFunc(void* userPtr,void* lsMemory)
{
@@ -127,18 +139,18 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
args->m_cs->unlock();
double deltaTimeInSeconds = 0;
do
{
//todo(erwincoumans): do we want some sleep to reduce CPU resources in this thread?
double deltaTimeInSeconds = double(clock.getTimeMicroseconds())/1000000.;
deltaTimeInSeconds+= double(clock.getTimeMicroseconds())/1000000.;
if (deltaTimeInSeconds<(1./5000.))
{
skip++;
skip1++;
if (skip1>5)
{
b3Clock::usleep(250);
}
@@ -149,40 +161,58 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
//process special controller commands, such as
//VR controller button press/release and controller motion
btVector3 from = args->m_pos;
btMatrix3x3 mat(args->m_orn);
btScalar pickDistance = 100.;
btVector3 toX = args->m_pos+mat.getColumn(0);
btVector3 toY = args->m_pos+mat.getColumn(1);
btVector3 toZ = args->m_pos+mat.getColumn(2)*pickDistance;
if (args->m_isPicking)
for (int c=0;c<MAX_VR_CONTROLLERS;c++)
{
args->m_isPicking = false;
args->m_isDragging = true;
args->m_physicsServerPtr->pickBody(from,-toZ);
//printf("PICK!\n");
}
if (args->m_isDragging)
{
args->m_physicsServerPtr->movePickedBody(from,-toZ);
// printf(".");
}
if (args->m_isReleasing)
{
args->m_isDragging = false;
args->m_isReleasing = false;
args->m_physicsServerPtr->removePickingConstraint();
//printf("Release pick\n");
btVector3 from = args->m_vrControllerPos[c];
btMatrix3x3 mat(args->m_vrControllerOrn[c]);
btScalar pickDistance = 1000.;
btVector3 toX = from+mat.getColumn(0);
btVector3 toY = from+mat.getColumn(1);
btVector3 toZ = from+mat.getColumn(2)*pickDistance;
if (args->m_isVrControllerTeleporting[c])
{
args->m_isVrControllerTeleporting[c] = false;
args->m_physicsServerPtr->pickBody(from,-toZ);
args->m_physicsServerPtr->removePickingConstraint();
}
if (args->m_isVrControllerPicking[c])
{
args->m_isVrControllerPicking[c] = false;
args->m_isVrControllerDragging[c] = true;
args->m_physicsServerPtr->pickBody(from,-toZ);
//printf("PICK!\n");
}
if (args->m_isVrControllerDragging[c])
{
args->m_physicsServerPtr->movePickedBody(from,-toZ);
// printf(".");
}
if (args->m_isVrControllerReleasing[c])
{
args->m_isVrControllerDragging[c] = false;
args->m_isVrControllerReleasing[c] = false;
args->m_physicsServerPtr->removePickingConstraint();
//printf("Release pick\n");
}
}
//don't simulate over a huge timestep if we had some interruption (debugger breakpoint etc)
btClamp(deltaTimeInSeconds,0.,0.1);
args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds);
if (deltaTimeInSeconds>clampedDeltaTime)
{
deltaTimeInSeconds = clampedDeltaTime;
b3Warning("Clamp deltaTime from %f to %f",deltaTimeInSeconds, clampedDeltaTime);
}
clock.reset();
args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds);
deltaTimeInSeconds = 0;
}
args->m_physicsServerPtr->processClientCommands();
@@ -317,7 +347,10 @@ public:
m_childGuiHelper->render(0);
}
virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld){}
virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld)
{
m_childGuiHelper->createPhysicsDebugDrawer(rbWorld);
}
virtual int registerTexture(const unsigned char* texels, int width, int height)
{
@@ -844,61 +877,101 @@ void PhysicsServerExample::stepSimulation(float deltaTime)
}
}
static float vrOffset[16]={1,0,0,0,
0,1,0,0,
0,0,1,0,
0,0,0,0};
extern int gDroppedSimulationSteps;
extern int gNumSteps;
extern double gDtInSec;
extern double gSubStep;
void PhysicsServerExample::renderScene()
{
///debug rendering
//m_args[0].m_cs->lock();
vrOffset[12]=-gVRTeleportPos[0];
vrOffset[13]=-gVRTeleportPos[1];
vrOffset[14]=-gVRTeleportPos[2];
this->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->
getActiveCamera()->setVRCameraOffsetTransform(vrOffset);
m_physicsServer.renderScene();
if (m_args[0].m_isPicking || m_args[0].m_isDragging)
for (int i=0;i<MAX_VR_CONTROLLERS;i++)
{
btVector3 from = m_args[0].m_pos;
btMatrix3x3 mat(m_args[0].m_orn);
if (m_args[0].m_isVrControllerPicking[i] || m_args[0].m_isVrControllerDragging[i])
{
btVector3 from = m_args[0].m_vrControllerPos[i];
btMatrix3x3 mat(m_args[0].m_vrControllerOrn[i]);
btVector3 toX = m_args[0].m_pos+mat.getColumn(0);
btVector3 toY = m_args[0].m_pos+mat.getColumn(1);
btVector3 toZ = m_args[0].m_pos+mat.getColumn(2);
btVector3 toX = from+mat.getColumn(0);
btVector3 toY = from+mat.getColumn(1);
btVector3 toZ = from+mat.getColumn(2);
int width = 2;
int width = 2;
btVector4 color;
color=btVector4(1,0,0,1);
m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toX,color,width);
color=btVector4(0,1,0,1);
m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toY,color,width);
color=btVector4(0,0,1,1);
m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toZ,color,width);
btVector4 color;
color=btVector4(1,0,0,1);
m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toX,color,width);
color=btVector4(0,1,0,1);
m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toY,color,width);
color=btVector4(0,0,1,1);
m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toZ,color,width);
}
}
if (gDebugRenderToggle)
if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
{
//some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift)
static int frameCount=0;
frameCount++;
char bla[1024];
static btScalar prevTime = m_clock.getTimeSeconds();
btScalar curTime = m_clock.getTimeSeconds();
static btScalar deltaTime = 0.f;
static int count = 10;
if (count-- < 0)
{
count = 10;
deltaTime = curTime - prevTime;
}
if (deltaTime == 0)
deltaTime = 1000;
frameCount++;
static char line0[1024];
static char line1[1024];
static btScalar worseFps = 1000000;
int numFrames = 200;
static int count = 0;
count++;
if (0 == (count & 1))
{
btScalar curTime = m_clock.getTimeSeconds();
btScalar fps = 1. / (curTime - prevTime);
prevTime = curTime;
if (fps < worseFps)
{
worseFps = fps;
}
if (count > numFrames)
{
count = 0;
sprintf(line0, "Graphics FPS (worse) = %f, frame %d", worseFps, frameCount / 2);
sprintf(line1, "Physics Steps = %d, Dropped = %d, dt %f, Substep %f)", gNumSteps, gDroppedSimulationSteps, gDtInSec, gSubStep);
gDroppedSimulationSteps = 0;
worseFps = 1000000;
}
}
prevTime = curTime;
sprintf(bla,"VR sub-title text test,fps = %f, frame %d", 1./deltaTime, frameCount/2);
float pos[4];
m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraTargetPosition(pos);
pos[0]+=gVRTeleportPos[0];
pos[1]+=gVRTeleportPos[1];
pos[2]+=gVRTeleportPos[2];
btTransform viewTr;
btScalar m[16];
float mf[16];
@@ -907,17 +980,20 @@ void PhysicsServerExample::renderScene()
{
m[i] = mf[i];
}
m[12]=+gVRTeleportPos[0];
m[13]=+gVRTeleportPos[1];
m[14]=+gVRTeleportPos[2];
viewTr.setFromOpenGLMatrix(m);
btTransform viewTrInv = viewTr.inverse();
float upMag = -.6;
btVector3 side = viewTrInv.getBasis().getColumn(0);
btVector3 up = viewTrInv.getBasis().getColumn(1);
up+=0.35*side;
m_guiHelper->getAppInterface()->drawText3D(bla,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1);
up+=0.6*side;
m_guiHelper->getAppInterface()->drawText3D(line0,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1);
//btVector3 fwd = viewTrInv.getBasis().getColumn(2);
sprintf(bla,"VR line 2 sub-title text test, frame %d", frameCount/2);
upMag = -0.7;
m_guiHelper->getAppInterface()->drawText3D(bla,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1);
m_guiHelper->getAppInterface()->drawText3D(line1,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1);
}
//m_args[0].m_cs->unlock();
@@ -951,7 +1027,7 @@ btVector3 PhysicsServerExample::getRayTo(int x,int y)
btVector3 camPos,camTarget;
renderer->getActiveCamera()->getCameraPosition(camPos);
renderer->getActiveCamera()->getCameraTargetPosition(camTarget);
btVector3 rayFrom = camPos;
btVector3 rayForward = (camTarget-camPos);
rayForward.normalize();
@@ -1024,17 +1100,72 @@ class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOpt
}
void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orn[4])
{
m_args[0].m_isPicking = (state!=0);
m_args[0].m_isReleasing = (state==0);
m_args[0].m_pos.setValue(pos[0],pos[1],pos[2]);
m_args[0].m_orn.setValue(orn[0],orn[1],orn[2],orn[3]);
//printf("controllerId %d, button=%d\n",controllerId, button);
if (controllerId<0 || controllerId>=MAX_VR_CONTROLLERS)
return;
if (button==1 && state==0)
{
gVRTeleportPos = gLastPickPos;
}
if (button==32 && state==0)
{
gDebugRenderToggle = !gDebugRenderToggle;
}
if (button==1)
{
m_args[0].m_isVrControllerTeleporting[controllerId] = true;
}
if (controllerId == 3 && (button == 33))
{
gVRGripperClosed =state;
}
else
{
if ((button == 33))
{
m_args[0].m_isVrControllerPicking[controllerId] = (state != 0);
m_args[0].m_isVrControllerReleasing[controllerId] = (state == 0);
}
if ((button == 33) || (button == 1))
{
m_args[0].m_vrControllerPos[controllerId].setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0], orn[1], orn[2], orn[3]);
}
}
}
extern btVector3 gVRGripperPos;
extern btQuaternion gVRGripperOrn;
void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4])
{
m_args[0].m_pos.setValue(pos[0],pos[1],pos[2]);
m_args[0].m_orn.setValue(orn[0],orn[1],orn[2],orn[3]);
if (controllerId <= 0 || controllerId >= MAX_VR_CONTROLLERS)
{
printf("Controller Id exceeds max: %d > %d", controllerId, MAX_VR_CONTROLLERS);
return;
}
if (controllerId == 3)
{
gVRGripperPos.setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
btQuaternion orgOrn(orn[0], orn[1], orn[2], orn[3]);
gVRGripperOrn = orgOrn*btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI);
}
else
{
m_args[0].m_vrControllerPos[controllerId].setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0], orn[1], orn[2], orn[3]);
}
}
B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc)

19
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -223,6 +223,7 @@ enum EnumSimParamUpdateFlags
SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS=4,
SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS=8,
SIM_PARAM_UPDATE_REAL_TIME_SIMULATION = 16,
SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP=32
};
///Controlling a robot involves sending the desired state to its joint motor controllers.
@@ -234,6 +235,7 @@ struct SendPhysicsSimulationParameters
int m_numSimulationSubSteps;
int m_numSolverIterations;
bool m_allowRealTimeSimulation;
double m_defaultContactERP;
};
struct RequestActualStateArgs
@@ -389,6 +391,21 @@ struct CalculateJacobianResultArgs
double m_angularJacobian[3*MAX_DEGREE_OF_FREEDOM];
};
struct CalculateInverseKinematicsArgs
{
int m_bodyUniqueId;
double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
double m_targetPosition[3];
double m_targetOrientation[4];//orientation represented as quaternion, x,y,z,w
};
struct CalculateInverseKinematicsResultArgs
{
int m_bodyUniqueId;
int m_dofCount;
double m_jointPositions[MAX_DEGREE_OF_FREEDOM];
};
struct CreateJointArgs
{
int m_parentBodyIndex;
@@ -431,6 +448,7 @@ struct SharedMemoryCommand
struct CalculateJacobianArgs m_calculateJacobianArguments;
struct CreateJointArgs m_createJointArguments;
struct RequestContactDataArgs m_requestContactPointArguments;
struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments;
};
};
@@ -464,6 +482,7 @@ struct SharedMemoryStatus
struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs;
struct CalculateJacobianResultArgs m_jacobianResultArgs;
struct SendContactDataArgs m_sendContactPointArgs;
struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;
};
};

2
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -72,6 +72,8 @@ enum EnumSharedMemoryServerStatus
CMD_CALCULATED_JACOBIAN_FAILED,
CMD_CONTACT_POINT_INFORMATION_COMPLETED,
CMD_CONTACT_POINT_INFORMATION_FAILED,
CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED,
CMD_CALCULATE_INVERSE_KINEMATICS_FAILED,
CMD_MAX_SERVER_COMMANDS
};

10
examples/SharedMemory/premake4.lua
View File

@@ -10,13 +10,15 @@ end
includedirs {".","../../src", "../ThirdPartyLibs",}
links {
"Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath"
"Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "BussIK"
}
language "C++"
myfiles =
{
"IKTrajectoryHelper.cpp",
"IKTrajectoryHelper.h",
"PhysicsClient.cpp",
"PhysicsClientSharedMemory.cpp",
"PhysicsClientExample.cpp",
@@ -134,10 +136,10 @@ else
end
defines {"B3_USE_STANDALONE_EXAMPLE"}
includedirs {"../../src"}
includedirs {"../../src", "../ThirdPartyLibs"}
links {
"BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common"
"BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common","BussIK"
}
initOpenGL()
initGlew()
@@ -211,7 +213,7 @@ if os.is("Windows") then
}
links {
"BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath","OpenGL_Window","openvr_api"
"BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath","OpenGL_Window","openvr_api","BussIK"
}