int main(int argc, char* argv[])
{
kPhysClient = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
if (!kPhysClient)
return -1;
// visualizer
command = b3InitConfigureOpenGLVisualizer(kPhysClient);
b3ConfigureOpenGLVisualizerSetVisualizationFlags(command, COV_ENABLE_GUI, 0);
b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
b3ConfigureOpenGLVisualizerSetVisualizationFlags(command, COV_ENABLE_SHADOWS, 0);
b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
b3SetTimeOut(kPhysClient, 10);
//syncBodies is only needed when connecting to an existing physics server that has already some bodies
command = b3InitSyncBodyInfoCommand(kPhysClient);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
statusType = b3GetStatusType(statusHandle);
// set fixed time step
command = b3InitPhysicsParamCommand(kPhysClient);
ret = b3PhysicsParamSetTimeStep(command, FIXED_TIMESTEP);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
ret = b3PhysicsParamSetRealTimeSimulation(command, false);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
// load test
command = b3LoadUrdfCommandInit(kPhysClient, "TwoJointRobot_wo_fixedJoints.urdf");
int flags = URDF_USE_INERTIA_FROM_FILE;
b3LoadUrdfCommandSetFlags(command, flags);
b3LoadUrdfCommandSetUseFixedBase(command, true);
// q.setEulerZYX(0, 0, 0);
// b3LoadUrdfCommandSetStartOrientation(command, q[0], q[1], q[2], q[3]);
b3LoadUrdfCommandSetUseMultiBody(command, true);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
statusType = b3GetStatusType(statusHandle);
b3Assert(statusType == CMD_URDF_LOADING_COMPLETED);
if (statusType == CMD_URDF_LOADING_COMPLETED)
{
twojoint = b3GetStatusBodyIndex(statusHandle);
}
//disable default linear/angular damping
b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo(kPhysClient);
double linearDamping = 0;
double angularDamping = 0;
b3ChangeDynamicsInfoSetLinearDamping(command, twojoint, linearDamping);
b3ChangeDynamicsInfoSetAngularDamping(command, twojoint, angularDamping);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
int numJoints = b3GetNumJoints(kPhysClient, twojoint);
printf("twojoint numjoints = %d\n", numJoints);
// Loop through all joints
for (int i = 0; i < numJoints; ++i)
{
b3GetJointInfo(kPhysClient, twojoint, i, &jointInfo);
if (jointInfo.m_jointName[0])
{
jointNameToId[std::string(jointInfo.m_jointName)] = i;
}
else
{
continue;
}
// Reset before torque control - see #1459
command = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_VELOCITY);
b3JointControlSetDesiredVelocity(command, jointInfo.m_uIndex, 0);
b3JointControlSetMaximumForce(command, jointInfo.m_uIndex, 0);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
}
// loop
unsigned long dtus1 = (unsigned long)1000000.0 * FIXED_TIMESTEP;
double simTimeS = 0;
double q[2], v[2];
while (b3CanSubmitCommand(kPhysClient))
{
simTimeS += 0.000001 * dtus1;
// apply some torque
b3GetJointInfo(kPhysClient, twojoint, jointNameToId["joint_2"], &jointInfo);
command = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_TORQUE);
b3JointControlSetDesiredForceTorque(command, jointInfo.m_uIndex, 0.5 * sin(10 * simTimeS));
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
// get joint values
command = b3RequestActualStateCommandInit(kPhysClient, twojoint);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
b3GetJointState(kPhysClient, statusHandle, jointNameToId["joint_1"], &state);
q[0] = state.m_jointPosition;
v[0] = state.m_jointVelocity;
b3GetJointState(kPhysClient, statusHandle, jointNameToId["joint_2"], &state);
q[1] = state.m_jointPosition;
v[1] = state.m_jointVelocity;
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, b3InitStepSimulationCommand(kPhysClient));
// debugging output
printf("%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", simTimeS, q[0], q[1], v[0], v[1]);
b3Clock::usleep(1000. * 1000. * FIXED_TIMESTEP);
}
b3DisconnectSharedMemory(kPhysClient);
return 0;
}
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode)
{
return b3JointControlCommandInit2(physClient,0,controlMode);
}
B3_SHARED_API int preTickPluginCallback_vrSyncPlugin(struct b3PluginContext* context)
{
MyClass* obj = (MyClass*)context->m_userPointer;
if (obj && obj->m_controllerId >= 0)
{
{
int i = 0;
{
for (int n = 0; n < context->m_numVRControllerEvents; n++)
{
const b3VRControllerEvent& event = context->m_vrControllerEvents[n];
if (event.m_controllerId == obj->m_controllerId)
{
if (obj->m_constraintId >= 0)
{
struct b3UserConstraint constraintInfo;
if (b3GetUserConstraintInfo(context->m_physClient, obj->m_constraintId, &constraintInfo))
{
//this is basically equivalent to doing this in Python/pybullet:
//p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=...)
b3SharedMemoryCommandHandle commandHandle;
int userConstraintUniqueId = obj->m_constraintId;
commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, userConstraintUniqueId);
double pos[4] = {event.m_pos[0], event.m_pos[1], event.m_pos[2], 1};
b3InitChangeUserConstraintSetPivotInB(commandHandle, pos);
double orn[4] = {event.m_orn[0], event.m_orn[1], event.m_orn[2], event.m_orn[3]};
b3InitChangeUserConstraintSetFrameInB(commandHandle, orn);
b3InitChangeUserConstraintSetMaxForce(commandHandle, obj->m_maxForce);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
}
}
// apply the analogue button to close the constraint, using a gear constraint with position target
if (obj->m_constraintId2 >= 0)
{
struct b3UserConstraint constraintInfo;
if (b3GetUserConstraintInfo(context->m_physClient, obj->m_constraintId2, &constraintInfo))
{
//this block is similar to
//p.changeConstraint(c,gearRatio=1, erp=..., relativePositionTarget=relPosTarget, maxForce=...)
//printf("obj->m_constraintId2=%d\n", obj->m_constraintId2);
b3SharedMemoryCommandHandle commandHandle;
commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, obj->m_constraintId2);
//0 -> open, 1 = closed
double openPos = 1.;
double relPosTarget = openPos - (event.m_analogAxis * openPos);
b3InitChangeUserConstraintSetRelativePositionTarget(commandHandle, relPosTarget);
b3InitChangeUserConstraintSetERP(commandHandle, 1);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
}
}
//printf("event.m_analogAxis=%f\n", event.m_analogAxis);
// use the pr2_gripper motors to keep the gripper centered/symmetric around the center axis
if (obj->m_gripperId >= 0)
{
//this block is similar to
//b = p.getJointState(pr2_gripper,2)[0]
//print("b = " + str(b))
//p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3)
//printf("obj->m_gripperId=%d\n", obj->m_gripperId);
{
b3SharedMemoryCommandHandle cmd_handle =
b3RequestActualStateCommandInit(context->m_physClient, obj->m_gripperId);
b3SharedMemoryStatusHandle status_handle =
b3SubmitClientCommandAndWaitStatus(context->m_physClient, cmd_handle);
int status_type = b3GetStatusType(status_handle);
if (status_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
//printf("status_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED\n");
b3JointSensorState sensorState;
if (b3GetJointState(context->m_physClient, status_handle, 2, &sensorState))
{
b3SharedMemoryCommandHandle commandHandle;
double targetPosition = sensorState.m_jointPosition;
//printf("targetPosition =%f\n", targetPosition);
if (1)
{
b3JointInfo info;
b3GetJointInfo(context->m_physClient, obj->m_gripperId, 0, &info);
commandHandle = b3JointControlCommandInit2(context->m_physClient, obj->m_gripperId, CONTROL_MODE_POSITION_VELOCITY_PD);
double kp = .1;
double targetVelocity = 0;
double kd = .6;
b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition);
b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, obj->m_maxForce2);
b3SubmitClientCommandAndWaitStatus(context->m_physClient, cmd_handle);
}
}
else
{
//printf("???\n");
}
}
else
{
//printf("no\n");
}
}
}
}
}
}
}
}
return 0;
}
void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
{
switch (commandId)
{
case CMD_LOAD_URDF:
{
b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf");
//setting the initial position, orientation and other arguments are optional
double startPosX = 0;
static double startPosY = 0;
double startPosZ = 0;
b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX, startPosY, startPosZ);
startPosY += 2.f;
// ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_LOAD_SDF:
{
#ifdef BT_DEBUG
b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf");
#else
b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "kitchens/1.sdf"); //two_cubes.sdf");//kitchens/1.sdf");//kuka_iiwa/model.sdf");
#endif
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_REQUEST_CAMERA_IMAGE_DATA:
{
///request an image from a simulated camera, using a software renderer.
b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle);
//b3RequestCameraImageSelectRenderer(commandHandle,ER_BULLET_HARDWARE_OPENGL);
float viewMatrix[16];
float projectionMatrix[16];
m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix);
m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix);
b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix, projectionMatrix);
b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth, camVisualizerHeight);
float lightPos[3];
lightPos[0] = m_lightPos[0];
lightPos[1] = m_lightPos[1];
lightPos[2] = m_lightPos[2];
b3RequestCameraImageSetLightDirection(commandHandle, lightPos);
b3RequestCameraImageSetLightSpecularCoeff(commandHandle, m_specularCoeff);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle);
b3CreateBoxCommandSetStartPosition(commandHandle, 0, 0, -1.5);
b3CreateBoxCommandSetColorRGBA(commandHandle, 0, 0, 1, 1);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_CREATE_RIGID_BODY:
{
b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle);
b3CreateBoxCommandSetStartPosition(commandHandle, 0, 0, 0);
b3CreateBoxCommandSetMass(commandHandle, 1);
b3CreateBoxCommandSetCollisionShapeType(commandHandle, COLLISION_SHAPE_TYPE_CYLINDER_Y);
b3CreateBoxCommandSetColorRGBA(commandHandle, 1, 1, 0, 1);
double radius = 0.2;
double halfHeight = 0.5;
b3CreateBoxCommandSetHalfExtents(commandHandle, radius, halfHeight, radius);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_REQUEST_ACTUAL_STATE:
{
if (m_selectedBody >= 0)
{
b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle, m_selectedBody);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_physicsClientHandle, commandHandle);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody);
for (int i = 0; i < numJoints; ++i)
{
struct b3JointSensorState sensorState;
b3GetJointState(m_physicsClientHandle, statusHandle, i, &sensorState);
//b3Printf("Joint %d: %f", i, sensorState.m_jointMotorTorque);
}
}
break;
};
case CMD_INIT_POSE:
{
if (m_selectedBody >= 0)
{
b3SharedMemoryCommandHandle commandHandle = b3CreatePoseCommandInit(m_physicsClientHandle, m_selectedBody);
static int toggle = 0;
double pos[3] = {0, 0, 0};
pos[toggle] = 2;
toggle++;
if (toggle > 2)
toggle = 0;
btQuaternion orn;
orn.setValue(0, 0, 0, 1);
switch (toggle)
{
case 0:
orn = btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI);
break;
case 1:
orn = btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI);
break;
case 2:
orn = btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI);
break;
default:
orn.setValue(0, 0, 0, 1);
};
b3CreatePoseCommandSetBaseOrientation(commandHandle, orn[0], orn[1], orn[2], orn[3]);
b3CreatePoseCommandSetBasePosition(commandHandle, pos[0], pos[1], pos[2]);
int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody);
static double jointPos = SIMD_PI / 2.f;
for (int i = 0; i < numJoints; i++)
{
b3JointInfo info;
b3GetJointInfo(m_physicsClientHandle, m_selectedBody, i, &info);
if ((info.m_jointType == 0) || (info.m_jointType == 1)) //revolute or prismatic
{
b3CreatePoseCommandSetJointPosition(m_physicsClientHandle, commandHandle, i, jointPos);
}
}
jointPos += SIMD_PI / 8.0;
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
}
break;
}
case CMD_STEP_FORWARD_SIMULATION:
{
b3SharedMemoryCommandHandle commandHandle = b3InitStepSimulationCommand(m_physicsClientHandle);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_REQUEST_DEBUG_LINES:
{
b3SharedMemoryCommandHandle commandHandle = b3InitRequestDebugLinesCommand(m_physicsClientHandle, btIDebugDraw::DBG_DrawWireframe);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_SEND_DESIRED_STATE:
{
if (m_selectedBody >= 0)
{
// b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_VELOCITY);
b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_physicsClientHandle, m_selectedBody, CONTROL_MODE_POSITION_VELOCITY_PD);
// b3Printf("prepare control command for body %d", m_selectedBody);
prepareControlCommand(command);
b3SubmitClientCommand(m_physicsClientHandle, command);
}
break;
}
case CMD_RESET_SIMULATION:
{
b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_SEND_BULLET_DATA_STREAM:
{
#if 0
//this worked, but needs C-API and a streaming options, similar to debug lines
command.m_type = buttonId;
cl->enqueueCommand(command);
#endif
break;
}
case CMD_CUSTOM_SET_GRAVITY:
{
b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle);
b3PhysicsParamSetGravity(commandHandle, 0.0, 0.0, -9.8);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_CUSTOM_SET_REALTIME_SIMULATION:
{
b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle);
b3PhysicsParamSetRealTimeSimulation(commandHandle, 1);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
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;
}
case CMD_REQUEST_CONTACT_POINT_INFORMATION:
{
b3SharedMemoryCommandHandle commandHandle = b3InitRequestContactPointInformation(m_physicsClientHandle);
b3SetContactFilterBodyA(commandHandle, 0);
b3SetContactFilterBodyB(commandHandle, 1);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_SAVE_WORLD:
{
b3SharedMemoryCommandHandle commandHandle = b3SaveWorldCommandInit(m_physicsClientHandle, "saveWorld.py");
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_REQUEST_VISUAL_SHAPE_INFO:
{
if (m_selectedBody >= 0)
{
//request visual shape information
b3SharedMemoryCommandHandle commandHandle = b3InitRequestVisualShapeInformation(m_physicsClientHandle, m_selectedBody);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
}
break;
}
case CMD_SET_SHADOW:
{
b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle);
float viewMatrix[16];
float projectionMatrix[16];
m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix);
m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix);
b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix, projectionMatrix);
b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth, camVisualizerHeight);
bool hasShadow = true;
b3RequestCameraImageSetShadow(commandHandle, hasShadow);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_UPDATE_VISUAL_SHAPE:
{
int objectUniqueId = 0;
int linkIndex = -1;
int shapeIndex = -1;
int textureIndex = -2;
double rgbaColor[4] = {0.0, 1.0, 0.0, 1.0};
b3SharedMemoryCommandHandle commandHandle = b3InitUpdateVisualShape2(m_physicsClientHandle, objectUniqueId, linkIndex, shapeIndex);
b3UpdateVisualShapeRGBAColor(commandHandle, rgbaColor);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
default:
{
b3Error("Unknown buttonId");
btAssert(0);
}
};
}
