TorqueGameEngines/Torque3D
1
0
Fork 0
mirror of https://github.com/TorqueGameEngines/Torque3D.git synced 2026-07-14 08:04:40 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

* Adjustment: Update Bullet version to 3.24.

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

161
Engine/lib/bullet/examples/RobotSimulator/CMakeLists.txt Normal file
View file

@ -0,0 +1,161 @@
INCLUDE_DIRECTORIES(
${BULLET_PHYSICS_SOURCE_DIR}/src
${BULLET_PHYSICS_SOURCE_DIR}/examples
${BULLET_PHYSICS_SOURCE_DIR}/examples/SharedMemory
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/enet/include
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/clsocket/src
)
SET(RobotSimulator_SRCS
RobotSimulatorMain.cpp
b3RobotSimulatorClientAPI.cpp
b3RobotSimulatorClientAPI.h
MinitaurSetup.cpp
MinitaurSetup.h
../../examples/ExampleBrowser/InProcessExampleBrowser.cpp
../SharedMemory/GraphicsServerExample.cpp
../SharedMemory/GraphicsClientExample.cpp
../SharedMemory/RemoteGUIHelper.cpp
../SharedMemory/RemoteGUIHelperTCP.cpp
../SharedMemory/GraphicsServerExample.h
../SharedMemory/GraphicsClientExample.h
../SharedMemory/RemoteGUIHelper.h
../SharedMemory/GraphicsSharedMemoryCommands.h
../SharedMemory/GraphicsSharedMemoryPublic.h
../../examples/SharedMemory/PhysicsServerExample.cpp
../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp
../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
)
IF(BUILD_CLSOCKET)
ADD_DEFINITIONS(-DBT_ENABLE_CLSOCKET)
ADD_DEFINITIONS(${OSDEF})
ENDIF(BUILD_CLSOCKET)
#some code to support OpenGL and Glew cross platform
IF (WIN32)
INCLUDE_DIRECTORIES(
${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows
)
ADD_DEFINITIONS(-DGLEW_STATIC)
LINK_LIBRARIES( ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} )
ELSE(WIN32)
IF(APPLE)
find_library(COCOA NAMES Cocoa)
MESSAGE(${COCOA})
link_libraries(${COCOA} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY})
ELSE(APPLE)
INCLUDE_DIRECTORIES(
${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows
)
ADD_DEFINITIONS("-DGLEW_INIT_OPENGL11_FUNCTIONS=1")
ADD_DEFINITIONS("-DGLEW_STATIC")
ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1")
FIND_PACKAGE(Threads)
LINK_LIBRARIES( ${CMAKE_THREAD_LIBS_INIT} ${DL} )
ENDIF(APPLE)
ENDIF(WIN32)
ADD_EXECUTABLE(App_RobotSimulator ${RobotSimulator_SRCS})
SET_TARGET_PROPERTIES(App_RobotSimulator PROPERTIES VERSION ${BULLET_VERSION})
SET_TARGET_PROPERTIES(App_RobotSimulator PROPERTIES DEBUG_POSTFIX "_d")
SET_TARGET_PROPERTIES(App_RobotSimulator PROPERTIES COMPILE_DEFINITIONS "B3_USE_ROBOTSIM_GUI")
TARGET_LINK_LIBRARIES(App_RobotSimulator BulletRobotics BulletExampleBrowserLib BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen Bullet3Common)
IF(WIN32)
IF(BUILD_ENET OR BUILD_CLSOCKET)
TARGET_LINK_LIBRARIES(App_RobotSimulator ws2_32 Winmm)
ENDIF(BUILD_ENET OR BUILD_CLSOCKET)
ENDIF(WIN32)
INCLUDE_DIRECTORIES(
${BULLET_PHYSICS_SOURCE_DIR}/src
${BULLET_PHYSICS_SOURCE_DIR}/examples
${BULLET_PHYSICS_SOURCE_DIR}/examples/SharedMemory
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/enet/include
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/clsocket/src
)
SET(RobotSimulator_NoGUI_SRCS
RobotSimulatorMain.cpp
MinitaurSetup.cpp
MinitaurSetup.h
)
ADD_EXECUTABLE(App_RobotSimulator_NoGUI ${RobotSimulator_NoGUI_SRCS})
SET_TARGET_PROPERTIES(App_RobotSimulator_NoGUI PROPERTIES VERSION ${BULLET_VERSION})
SET_TARGET_PROPERTIES(App_RobotSimulator_NoGUI PROPERTIES DEBUG_POSTFIX "_d")
TARGET_LINK_LIBRARIES(App_RobotSimulator_NoGUI BulletRobotics BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath Bullet3Common)
IF(WIN32)
IF(BUILD_ENET OR BUILD_CLSOCKET)
TARGET_LINK_LIBRARIES(App_RobotSimulator_NoGUI ws2_32 Winmm)
ENDIF(BUILD_ENET OR BUILD_CLSOCKET)
ELSE()
IF(APPLE)
ELSE(APPLE)
FIND_PACKAGE(Threads)
LINK_LIBRARIES( ${CMAKE_THREAD_LIBS_INIT} ${DL} )
ENDIF(APPLE)
ENDIF(WIN32)
########
INCLUDE_DIRECTORIES(
${BULLET_PHYSICS_SOURCE_DIR}/src
${BULLET_PHYSICS_SOURCE_DIR}/examples
${BULLET_PHYSICS_SOURCE_DIR}/examples/SharedMemory
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/enet/include
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/clsocket/src
)
SET(App_HelloBulletRobotics_SRCS
HelloBulletRobotics.cpp
)
ADD_EXECUTABLE(App_HelloBulletRobotics ${App_HelloBulletRobotics_SRCS})
SET_TARGET_PROPERTIES(App_HelloBulletRobotics PROPERTIES VERSION ${BULLET_VERSION})
SET_TARGET_PROPERTIES(App_HelloBulletRobotics PROPERTIES DEBUG_POSTFIX "_d")
TARGET_LINK_LIBRARIES(App_HelloBulletRobotics BulletRobotics BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath Bullet3Common)
IF(WIN32)
IF(BUILD_ENET OR BUILD_CLSOCKET)
TARGET_LINK_LIBRARIES(App_HelloBulletRobotics ws2_32 Winmm)
ENDIF(BUILD_ENET OR BUILD_CLSOCKET)
ELSE()
IF(NOT APPLE)
FIND_PACKAGE(Threads)
TARGET_LINK_LIBRARIES( App_HelloBulletRobotics ${CMAKE_THREAD_LIBS_INIT} ${DL} )
ENDIF(NOT APPLE)
ENDIF(WIN32)
IF(WIN32)
TARGET_LINK_LIBRARIES(App_RobotSimulator ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY})
ENDIF(WIN32)

48
Engine/lib/bullet/examples/RobotSimulator/HelloBulletRobotics.cpp Normal file
View file

@ -0,0 +1,48 @@
#include "b3RobotSimulatorClientAPI_NoGUI.h"
int main(int argc, char* argv[])
{
b3RobotSimulatorClientAPI_NoGUI* sim = new b3RobotSimulatorClientAPI_NoGUI();
bool isConnected = sim->connect(eCONNECT_SHARED_MEMORY);
if (!isConnected)
{
printf("Using Direct mode\n");
isConnected = sim->connect(eCONNECT_DIRECT);
}
else
{
printf("Using shared memory\n");
}
//remove all existing objects (if any)
sim->resetSimulation();
sim->setGravity(btVector3(0, 0, -9.8));
sim->setNumSolverIterations(100);
b3RobotSimulatorSetPhysicsEngineParameters args;
sim->getPhysicsEngineParameters(args);
int planeUid = sim->loadURDF("plane.urdf");
printf("planeUid = %d\n", planeUid);
int r2d2Uid = sim->loadURDF("r2d2.urdf");
printf("r2d2 #joints = %d\n", sim->getNumJoints(r2d2Uid));
btVector3 basePosition = btVector3(0, 0, 0.5);
btQuaternion baseOrientation = btQuaternion(0, 0, 0, 1);
sim->resetBasePositionAndOrientation(r2d2Uid, basePosition, baseOrientation);
while (sim->isConnected())
{
btVector3 basePos;
btQuaternion baseOrn;
sim->getBasePositionAndOrientation(r2d2Uid, basePos, baseOrn);
printf("r2d2 basePosition = [%f,%f,%f]\n", basePos[0], basePos[1], basePos[2]);
sim->stepSimulation();
}
delete sim;
}

286
Engine/lib/bullet/examples/RobotSimulator/MinitaurSetup.cpp Normal file
View file

@ -0,0 +1,286 @@
#include "MinitaurSetup.h"
#include "b3RobotSimulatorClientAPI_NoGUI.h"
#include "Bullet3Common/b3HashMap.h"
struct MinitaurSetupInternalData
{
int m_quadrupedUniqueId;
MinitaurSetupInternalData()
: m_quadrupedUniqueId(-1)
{
}
b3HashMap<b3HashString, int> m_jointNameToId;
};
MinitaurSetup::MinitaurSetup()
{
m_data = new MinitaurSetupInternalData();
}
MinitaurSetup::~MinitaurSetup()
{
delete m_data;
}
void MinitaurSetup::setDesiredMotorAngle(class b3RobotSimulatorClientAPI_NoGUI* sim, const char* motorName, double desiredAngle, double maxTorque, double kp, double kd)
{
b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD);
controlArgs.m_maxTorqueValue = maxTorque;
controlArgs.m_kd = kd;
controlArgs.m_kp = kp;
controlArgs.m_targetPosition = desiredAngle;
sim->setJointMotorControl(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[motorName], controlArgs);
}
//pick exactly 1 configuration of the following
#define MINITAUR_RAINBOWDASH_V1
//#define MINITAUR_RAINBOWDASH_V0
//#define MINITAUR_V0
#if defined(MINITAUR_RAINBOWDASH_V1)
#define MINITAUR_HAS_DEFORMABLE_BRACKETS
static const char* minitaurURDF = "quadruped/minitaur_rainbow_dash_v1.urdf";
static const char* kneeNames[] = {
"knee_front_leftL_joint", //1
"knee_front_leftR_joint", //3
"knee_back_leftL_joint", //5
"knee_back_leftR_joint", //7
"knee_front_rightL_joint", //9
"knee_back_rightL_joint", //10
"knee_back_rightR_joint", //13
"knee_front_rightR_joint", //15
};
static const char* motorNames[] = {
"motor_front_leftL_joint", //0
"knee_front_leftL_joint", //1
"motor_front_leftR_joint", //2
"knee_front_leftR_joint", //3
"motor_back_leftL_joint", //4
"knee_back_leftL_joint", //5
"motor_back_leftR_joint", //6
"knee_back_leftR_joint", //7
"motor_front_rightL_joint", //8
"knee_front_rightL_joint", //9
"knee_back_rightL_joint", //10
"motor_back_rightL_joint", //11
"motor_back_rightR_joint", //12
"knee_back_rightR_joint", //13
"motor_front_rightR_joint", //14
"knee_front_rightR_joint", //15
};
static const char* bracketNames[] = {
"motor_front_rightR_bracket_joint",
"motor_front_leftL_bracket_joint",
"motor_back_rightR_bracket_joint",
"motor_back_leftL_bracket_joint",
};
static btVector3 KNEE_CONSTRAINT_POINT_LONG = btVector3(0, 0.0045, 0.088);
static btVector3 KNEE_CONSTRAINT_POINT_SHORT = btVector3(0, 0.0045, 0.100);
#elif defined(MINITAUR_RAINBOWDASH_V0)
static const char* minitaurURDF = "quadruped/minitaur_rainbow_dash.urdf";
static const char* kneeNames[] = {
"knee_front_leftL_joint", //1
"knee_front_leftR_joint", //3
"knee_back_leftL_joint", //5
"knee_back_leftR_joint", //7
"knee_front_rightL_joint", //9
"knee_back_rightL_joint", //10
"knee_back_rightR_joint", //13
"knee_front_rightR_joint", //15
};
static const char* motorNames[] = {
"motor_front_leftL_joint", //0
"knee_front_leftL_joint", //1
"motor_front_leftR_joint", //2
"knee_front_leftR_joint", //3
"motor_back_leftL_joint", //4
"knee_back_leftL_joint", //5
"motor_back_leftR_joint", //6
"knee_back_leftR_joint", //7
"motor_front_rightL_joint", //8
"knee_front_rightL_joint", //9
"knee_back_rightL_joint", //10
"motor_back_rightL_joint", //11
"motor_back_rightR_joint", //12
"knee_back_rightR_joint", //13
"motor_front_rightR_joint", //14
"knee_front_rightR_joint", //15
};
static btVector3 KNEE_CONSTRAINT_POINT_LONG = btVector3(0, 0.0045, 0.088);
static btVector3 KNEE_CONSTRAINT_POINT_SHORT = btVector3(0, 0.0045, 0.100);
#elif defined(MINITAUR_V0)
static const char* minitaurURDF = "quadruped/minitaur.urdf";
static const char* kneeNames[] = {
"knee_front_leftL_link",
"knee_front_leftR_link",
"knee_back_leftL_link",
"knee_back_leftR_link",
"knee_front_rightL_link",
"knee_back_rightL_link",
"knee_back_rightR_link",
"knee_front_rightR_link",
};
static const char* motorNames[] = {
"motor_front_leftL_joint",
"knee_front_leftL_link",
"motor_front_leftR_joint",
"knee_front_leftR_link",
"motor_back_leftL_joint",
"knee_back_leftL_link",
"motor_back_leftR_joint",
"knee_back_leftR_link",
"motor_front_rightL_joint",
"knee_front_rightL_link",
"knee_back_rightL_link",
"motor_back_rightL_joint",
"motor_back_rightR_joint",
"knee_back_rightR_link",
"motor_front_rightR_joint",
"knee_front_rightR_link",
};
static btVector3 KNEE_CONSTRAINT_POINT_LONG = btVector3(0, 0.005, 0.2);
static btVector3 KNEE_CONSTRAINT_POINT_SHORT = btVector3(0, 0.01, 0.2);
#endif
void MinitaurSetup::resetPose(class b3RobotSimulatorClientAPI_NoGUI* sim)
{
//release all motors
int numJoints = sim->getNumJoints(m_data->m_quadrupedUniqueId);
for (int i = 0; i < numJoints; i++)
{
b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_maxTorqueValue = 0;
sim->setJointMotorControl(m_data->m_quadrupedUniqueId, i, controlArgs);
}
b3Scalar startAngle = B3_HALF_PI;
b3Scalar upperLegLength = 11.5;
b3Scalar lowerLegLength = 20;
b3Scalar kneeAngle = B3_PI + b3Acos(upperLegLength / lowerLegLength);
b3Scalar motorDirs[8] = {-1, -1, -1, -1, 1, 1, 1, 1};
b3JointInfo jointInfo;
jointInfo.m_jointType = ePoint2PointType;
//left front leg
sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[motorNames[0]], motorDirs[0] * startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[0]], motorDirs[0] * kneeAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[motorNames[2]], motorDirs[1] * startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[1]], motorDirs[1] * kneeAngle);
jointInfo.m_parentFrame[0] = KNEE_CONSTRAINT_POINT_SHORT[0];
jointInfo.m_parentFrame[1] = KNEE_CONSTRAINT_POINT_SHORT[1];
jointInfo.m_parentFrame[2] = KNEE_CONSTRAINT_POINT_SHORT[2];
jointInfo.m_childFrame[0] = KNEE_CONSTRAINT_POINT_LONG[0];
jointInfo.m_childFrame[1] = KNEE_CONSTRAINT_POINT_LONG[1];
jointInfo.m_childFrame[2] = KNEE_CONSTRAINT_POINT_LONG[2];
//jointInfo.m_parentFrame[0] = KNEE_CONSTRAINT_POINT_LONG[0]; jointInfo.m_parentFrame[1] = KNEE_CONSTRAINT_POINT_LONG[1]; jointInfo.m_parentFrame[2] = KNEE_CONSTRAINT_POINT_LONG[2];
//jointInfo.m_childFrame[0] = KNEE_CONSTRAINT_POINT_SHORT[0]; jointInfo.m_childFrame[1] = KNEE_CONSTRAINT_POINT_SHORT[1]; jointInfo.m_childFrame[2] = KNEE_CONSTRAINT_POINT_SHORT[2];
sim->createConstraint(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[1]],
m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[0]], &jointInfo);
setDesiredMotorAngle(sim, motorNames[0], motorDirs[0] * startAngle);
setDesiredMotorAngle(sim, motorNames[2], motorDirs[1] * startAngle);
//left back leg
sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[motorNames[4]], motorDirs[2] * startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[2]], motorDirs[2] * kneeAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[motorNames[6]], motorDirs[3] * startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[3]], motorDirs[3] * kneeAngle);
jointInfo.m_parentFrame[0] = KNEE_CONSTRAINT_POINT_SHORT[0];
jointInfo.m_parentFrame[1] = KNEE_CONSTRAINT_POINT_SHORT[1];
jointInfo.m_parentFrame[2] = KNEE_CONSTRAINT_POINT_SHORT[2];
jointInfo.m_childFrame[0] = KNEE_CONSTRAINT_POINT_LONG[0];
jointInfo.m_childFrame[1] = KNEE_CONSTRAINT_POINT_LONG[1];
jointInfo.m_childFrame[2] = KNEE_CONSTRAINT_POINT_LONG[2];
//jointInfo.m_parentFrame[0] = KNEE_CONSTRAINT_POINT_LONG[0]; jointInfo.m_parentFrame[1] = KNEE_CONSTRAINT_POINT_LONG[1]; jointInfo.m_parentFrame[2] = KNEE_CONSTRAINT_POINT_LONG[2];
//jointInfo.m_childFrame[0] = KNEE_CONSTRAINT_POINT_SHORT[0]; jointInfo.m_childFrame[1] = KNEE_CONSTRAINT_POINT_SHORT[1]; jointInfo.m_childFrame[2] = KNEE_CONSTRAINT_POINT_SHORT[2];
sim->createConstraint(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[3]],
m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[2]], &jointInfo);
setDesiredMotorAngle(sim, motorNames[4], motorDirs[2] * startAngle);
setDesiredMotorAngle(sim, motorNames[6], motorDirs[3] * startAngle);
//right front leg
sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[motorNames[8]], motorDirs[4] * startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[4]], motorDirs[4] * kneeAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[motorNames[14]], motorDirs[5] * startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[7]], motorDirs[5] * kneeAngle);
jointInfo.m_parentFrame[0] = KNEE_CONSTRAINT_POINT_LONG[0];
jointInfo.m_parentFrame[1] = KNEE_CONSTRAINT_POINT_LONG[1];
jointInfo.m_parentFrame[2] = KNEE_CONSTRAINT_POINT_LONG[2];
jointInfo.m_childFrame[0] = KNEE_CONSTRAINT_POINT_SHORT[0];
jointInfo.m_childFrame[1] = KNEE_CONSTRAINT_POINT_SHORT[1];
jointInfo.m_childFrame[2] = KNEE_CONSTRAINT_POINT_SHORT[2];
sim->createConstraint(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[7]],
m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[4]], &jointInfo);
setDesiredMotorAngle(sim, motorNames[8], motorDirs[4] * startAngle);
setDesiredMotorAngle(sim, motorNames[14], motorDirs[5] * startAngle);
//right back leg
sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[motorNames[11]], motorDirs[6] * startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[5]], motorDirs[6] * kneeAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[motorNames[12]], motorDirs[7] * startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[6]], motorDirs[7] * kneeAngle);
jointInfo.m_parentFrame[0] = KNEE_CONSTRAINT_POINT_LONG[0];
jointInfo.m_parentFrame[1] = KNEE_CONSTRAINT_POINT_LONG[1];
jointInfo.m_parentFrame[2] = KNEE_CONSTRAINT_POINT_LONG[2];
jointInfo.m_childFrame[0] = KNEE_CONSTRAINT_POINT_SHORT[0];
jointInfo.m_childFrame[1] = KNEE_CONSTRAINT_POINT_SHORT[1];
jointInfo.m_childFrame[2] = KNEE_CONSTRAINT_POINT_SHORT[2];
sim->createConstraint(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[6]],
m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[5]], &jointInfo);
setDesiredMotorAngle(sim, motorNames[11], motorDirs[6] * startAngle);
setDesiredMotorAngle(sim, motorNames[12], motorDirs[7] * startAngle);
#ifdef MINITAUR_HAS_DEFORMABLE_BRACKETS
b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_maxTorqueValue = 6;
controlArgs.m_kd = 1;
controlArgs.m_kp = 0;
controlArgs.m_targetPosition = 0;
for (int i = 0; i < 4; i++)
{
const char* bracketName = bracketNames[i];
int* bracketId = m_data->m_jointNameToId[bracketName];
sim->setJointMotorControl(m_data->m_quadrupedUniqueId, *bracketId, controlArgs);
}
#endif
}
int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI_NoGUI* sim, const btVector3& startPos, const btQuaternion& startOrn)
{
b3RobotSimulatorLoadUrdfFileArgs args;
args.m_startPosition = startPos;
args.m_startOrientation = startOrn;
m_data->m_quadrupedUniqueId = sim->loadURDF(minitaurURDF, args);
int numJoints = sim->getNumJoints(m_data->m_quadrupedUniqueId);
for (int i = 0; i < numJoints; i++)
{
b3JointInfo jointInfo;
sim->getJointInfo(m_data->m_quadrupedUniqueId, i, &jointInfo);
if (jointInfo.m_jointName[0])
{
m_data->m_jointNameToId.insert(jointInfo.m_jointName, i);
}
}
resetPose(sim);
return m_data->m_quadrupedUniqueId;
}

19
Engine/lib/bullet/examples/RobotSimulator/MinitaurSetup.h Normal file
View file

@ -0,0 +1,19 @@
#ifndef MINITAUR_SIMULATION_SETUP_H
#define MINITAUR_SIMULATION_SETUP_H
#include "LinearMath/btVector3.h"
#include "LinearMath/btQuaternion.h"
class MinitaurSetup
{
struct MinitaurSetupInternalData* m_data;
void resetPose(class b3RobotSimulatorClientAPI_NoGUI* sim);
public:
MinitaurSetup();
virtual ~MinitaurSetup();
int setupMinitaur(class b3RobotSimulatorClientAPI_NoGUI* sim, const class btVector3& startPos = btVector3(0, 0, 0), const class btQuaternion& startOrn = btQuaternion(0, 0, 0, 1));
void setDesiredMotorAngle(class b3RobotSimulatorClientAPI_NoGUI* sim, const char* motorName, double desiredAngle, double maxTorque = 3, double kp = 0.1, double kd = 0.9);
};
#endif //MINITAUR_SIMULATION_SETUP_H

147
Engine/lib/bullet/examples/RobotSimulator/MinitaurSimulatorExample.cpp Normal file
View file

@ -0,0 +1,147 @@
#include "MinitaurSimulatorExample.h"
#include "MinitaurSetup.h"
#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "../CommonInterfaces/CommonRenderInterface.h"
#include "../CommonInterfaces/CommonExampleInterface.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "../SharedMemory/PhysicsServerSharedMemory.h"
#include "../SharedMemory/SharedMemoryPublic.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "../SharedMemory/PhysicsClientC_API.h"
#include <string>
#include "../RobotSimulator/b3RobotSimulatorClientAPI.h"
#include "../Utils/b3Clock.h"
///quick demo showing the right-handed coordinate system and positive rotations around each axis
class MinitaurSimulatorExample : public CommonExampleInterface
{
CommonGraphicsApp* m_app;
GUIHelperInterface* m_guiHelper;
b3RobotSimulatorClientAPI m_robotSim;
int m_options;
double m_time;
btScalar m_gravityAccelerationZ;
MinitaurSetup m_minitaur;
int m_minitaurUid;
public:
MinitaurSimulatorExample(GUIHelperInterface* helper, int options)
: m_app(helper->getAppInterface()),
m_guiHelper(helper),
m_options(options),
m_gravityAccelerationZ(-10),
m_minitaurUid(-1)
{
m_app->setUpAxis(2);
}
virtual ~MinitaurSimulatorExample()
{
}
virtual void physicsDebugDraw(int debugDrawMode)
{
m_robotSim.debugDraw(debugDrawMode);
}
virtual void initPhysics()
{
int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER;
m_robotSim.setGuiHelper(m_guiHelper);
bool connected = m_robotSim.connect(mode);
//hide the perception camera views for rbd, depth and segmentation mask
m_robotSim.configureDebugVisualizer(COV_ENABLE_RGB_BUFFER_PREVIEW, 0);
m_robotSim.configureDebugVisualizer(COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0);
m_robotSim.configureDebugVisualizer(COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0);
b3Printf("robotSim connected = %d", connected);
SliderParams slider("Gravity", &m_gravityAccelerationZ);
slider.m_minVal = -10;
slider.m_maxVal = 10;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
//when in the debugger, don't crash when a command isn't processed immediately, give it 10 seconds
m_robotSim.setTimeOut(10);
m_robotSim.loadURDF("plane.urdf");
m_minitaurUid = m_minitaur.setupMinitaur(&m_robotSim, btVector3(0, 0, .3));
{
b3RobotSimulatorLoadUrdfFileArgs args;
args.m_startPosition.setValue(0, 0, 1);
args.m_startOrientation.setEulerZYX(0, 0, 0);
args.m_useMultiBody = true;
m_robotSim.loadURDF("cube_small.urdf", args);
}
}
virtual void exitPhysics()
{
m_robotSim.disconnect();
}
virtual void stepSimulation(float deltaTime)
{
m_robotSim.setGravity(btVector3(0, 0, m_gravityAccelerationZ));
m_robotSim.stepSimulation();
for (int i = 0; i < m_robotSim.getNumJoints(m_minitaurUid);i++)
{
b3JointSensorState state;
m_robotSim.getJointState(this->m_minitaurUid, i, &state);
}
b3JointStates2 states;
m_robotSim.getJointStates(m_minitaurUid, states);
}
virtual void renderScene()
{
m_robotSim.renderScene();
}
virtual bool mouseMoveCallback(float x, float y)
{
return m_robotSim.mouseMoveCallback(x, y);
}
virtual bool mouseButtonCallback(int button, int state, float x, float y)
{
return m_robotSim.mouseButtonCallback(button, state, x, y);
}
virtual bool keyboardCallback(int key, int state)
{
return false;
}
virtual void resetCamera()
{
float dist = 1.5;
float pitch = -10;
float yaw = 18;
float targetPos[3] = {-0.2, 0.8, 0.3};
if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
{
m_app->m_renderer->getActiveCamera()->setCameraDistance(dist);
m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch);
m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw);
m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0], targetPos[1], targetPos[2]);
}
}
};
class CommonExampleInterface* MinitaurSimulatorExampleCreateFunc(struct CommonExampleOptions& options)
{
return new MinitaurSimulatorExample(options.m_guiHelper, options.m_option);
}

22
Engine/lib/bullet/examples/RobotSimulator/MinitaurSimulatorExample.h Normal file
View file

@ -0,0 +1,22 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef MINITAUR_SIMULATOR_EXAMPLE_H
#define MINITAUR_SIMULATOR_EXAMPLE_H
class CommonExampleInterface* MinitaurSimulatorExampleCreateFunc(struct CommonExampleOptions& options);
#endif //MINITAUR_SIMULATOR_EXAMPLE_H

152
Engine/lib/bullet/examples/RobotSimulator/RobotSimulatorMain.cpp Normal file
View file

@ -0,0 +1,152 @@
#ifdef B3_USE_ROBOTSIM_GUI
#include "b3RobotSimulatorClientAPI.h"
#else
#include "b3RobotSimulatorClientAPI_NoGUI.h"
#endif
#include "../Utils/b3Clock.h"
#include <string.h>
#include <stdio.h>
#include <assert.h>
#define ASSERT_EQ(a, b) assert((a) == (b));
#include "MinitaurSetup.h"
int main(int argc, char* argv[])
{
#ifdef B3_USE_ROBOTSIM_GUI
b3RobotSimulatorClientAPI* sim = new b3RobotSimulatorClientAPI();
bool isConnected = sim->connect(eCONNECT_GUI);
#else
b3RobotSimulatorClientAPI_NoGUI* sim = new b3RobotSimulatorClientAPI_NoGUI();
bool isConnected = sim->connect(eCONNECT_DIRECT);
#endif
if (!isConnected)
{
printf("Cannot connect\n");
return -1;
}
//Can also use eCONNECT_DIRECT,eCONNECT_SHARED_MEMORY,eCONNECT_UDP,eCONNECT_TCP, for example:
//sim->connect(eCONNECT_UDP, "localhost", 1234);
sim->configureDebugVisualizer(COV_ENABLE_GUI, 0);
// sim->configureDebugVisualizer( COV_ENABLE_SHADOWS, 0);//COV_ENABLE_WIREFRAME
sim->setTimeOut(10);
//syncBodies is only needed when connecting to an existing physics server that has already some bodies
sim->syncBodies();
btScalar fixedTimeStep = 1. / 240.;
sim->setTimeStep(fixedTimeStep);
btQuaternion q = sim->getQuaternionFromEuler(btVector3(0.1, 0.2, 0.3));
btVector3 rpy;
rpy = sim->getEulerFromQuaternion(q);
sim->setGravity(btVector3(0, 0, -9.8));
//int blockId = sim->loadURDF("cube.urdf");
//b3BodyInfo bodyInfo;
//sim->getBodyInfo(blockId,&bodyInfo);
sim->loadURDF("plane.urdf");
MinitaurSetup minitaur;
int minitaurUid = minitaur.setupMinitaur(sim, btVector3(0, 0, .3));
//b3RobotSimulatorLoadUrdfFileArgs args;
//args.m_startPosition.setValue(2,0,1);
//int r2d2 = sim->loadURDF("r2d2.urdf",args);
//b3RobotSimulatorLoadFileResults sdfResults;
//if (!sim->loadSDF("two_cubes.sdf",sdfResults))
//{
// b3Warning("Can't load SDF!\n");
//}
b3Clock clock;
double startTime = clock.getTimeInSeconds();
double simWallClockSeconds = 20.;
#if 0
while (clock.getTimeInSeconds()-startTime < simWallClockSeconds)
{
sim->stepSimulation();
}
#endif
sim->setRealTimeSimulation(false);
int vidLogId = -1;
int minitaurLogId = -1;
int rotateCamera = 0;
while (sim->canSubmitCommand())
{
b3KeyboardEventsData keyEvents;
sim->getKeyboardEvents(&keyEvents);
if (keyEvents.m_numKeyboardEvents)
{
//printf("num key events = %d]\n", keyEvents.m_numKeyboardEvents);
//m_keyState is a flag combination of eButtonIsDown,eButtonTriggered, eButtonReleased
for (int i = 0; i < keyEvents.m_numKeyboardEvents; i++)
{
b3KeyboardEvent& e = keyEvents.m_keyboardEvents[i];
if (e.m_keyCode == '0')
{
if (e.m_keyState & eButtonTriggered)
{
if (vidLogId < 0)
{
vidLogId = sim->startStateLogging(STATE_LOGGING_VIDEO_MP4, "video.mp4");
}
else
{
sim->stopStateLogging(vidLogId);
vidLogId = -1;
}
}
}
if (e.m_keyCode == 'm')
{
if (minitaurLogId < 0 && e.m_keyState & eButtonTriggered)
{
minitaurLogId = sim->startStateLogging(STATE_LOGGING_MINITAUR, "simlog.bin");
}
if (minitaurLogId >= 0 && e.m_keyState & eButtonReleased)
{
sim->stopStateLogging(minitaurLogId);
minitaurLogId = -1;
}
}
if (e.m_keyCode == 'r' && e.m_keyState & eButtonTriggered)
{
rotateCamera = 1 - rotateCamera;
}
//printf("keyEvent[%d].m_keyCode = %d, state = %d\n", i,keyEvents.m_keyboardEvents[i].m_keyCode,keyEvents.m_keyboardEvents[i].m_keyState);
}
}
sim->stepSimulation();
if (rotateCamera)
{
static double yaw = 0;
double distance = 1;
yaw += 0.1;
btVector3 basePos;
btQuaternion baseOrn;
sim->getBasePositionAndOrientation(minitaurUid, basePos, baseOrn);
sim->resetDebugVisualizerCamera(distance, -20, yaw, basePos);
}
b3Clock::usleep(1000. * 1000. * fixedTimeStep);
}
printf("sim->disconnect\n");
sim->disconnect();
printf("delete sim\n");
delete sim;
printf("exit\n");
}

347
Engine/lib/bullet/examples/RobotSimulator/VRGloveSimulatorMain.cpp Normal file
View file

@ -0,0 +1,347 @@
//VR Glove hand simulator is a C++ conversion from the Python pybullet vrhand_vive_tracker.py
//For more details about the VR glove, see also https://docs.google.com/document/d/1_qwXJRBTGKmhktdBtVQ6zdOdxwud1K30jt0G5dkAr10/edit
#include "b3RobotSimulatorClientAPI.h"
#include "../Utils/b3Clock.h"
#include "Bullet3Common/b3CommandLineArgs.h"
#include <string.h>
#include <stdio.h>
#include <assert.h>
#include "serial/serial.h"
#include "../Importers/ImportURDFDemo/urdfStringSplit.h"
double convertSensor(int inputV, int minV, int maxV)
{
b3Clamp(inputV, minV, maxV);
double outVal = (double)inputV;
double b = (outVal - (double)minV) / float(maxV - minV);
return (b);
}
void setJointMotorPositionControl(b3RobotSimulatorClientAPI* sim, int obUid, int linkIndex, double targetPosition)
{
b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD);
controlArgs.m_maxTorqueValue = 50.;
controlArgs.m_targetPosition = targetPosition;
controlArgs.m_targetVelocity = 0;
sim->setJointMotorControl(obUid, linkIndex, controlArgs);
}
int main(int argc, char* argv[])
{
b3CommandLineArgs args(argc, argv);
std::string port = "COM9";
args.GetCmdLineArgument("port", port);
int baud = 115200;
args.GetCmdLineArgument("speed", baud);
std::string mode = "SHARED_MEMORY";
args.GetCmdLineArgument("mode", mode);
int disableGui = 0;
args.GetCmdLineArgument("disableGui", disableGui);
int disableShadows = 0;
args.GetCmdLineArgument("disableShadows", disableShadows);
int useKitchen = 0;
args.GetCmdLineArgument("useKitchen", useKitchen);
int deviceTypeFilter = VR_DEVICE_GENERIC_TRACKER;
args.GetCmdLineArgument("deviceTypeFilter", deviceTypeFilter);
printf("port=%s, speed=%d, connection mode=%s\n", port.c_str(), baud, mode.c_str());
b3RobotSimulatorClientAPI* sim = new b3RobotSimulatorClientAPI();
//Can also use eCONNECT_UDP,eCONNECT_TCP, for example: sim->connect(eCONNECT_UDP, "localhost", 1234);
if (mode == "GUI")
{
sim->connect(eCONNECT_GUI);
}
else
{
if (mode == "DIRECT")
{
sim->connect(eCONNECT_DIRECT);
}
else
{
sim->connect(eCONNECT_SHARED_MEMORY);
}
}
sim->setRealTimeSimulation(true);
sim->setInternalSimFlags(0);
sim->resetSimulation();
if (disableGui)
{
sim->configureDebugVisualizer(COV_ENABLE_GUI, 0);
}
if (disableShadows)
{
sim->configureDebugVisualizer(COV_ENABLE_SHADOWS, 0);
}
sim->setTimeOut(12345);
//syncBodies is only needed when connecting to an existing physics server that has already some bodies
sim->syncBodies();
b3Scalar fixedTimeStep = 1. / 240.;
sim->setTimeStep(fixedTimeStep);
b3Quaternion q = sim->getQuaternionFromEuler(b3MakeVector3(0.1, 0.2, 0.3));
b3Vector3 rpy;
rpy = sim->getEulerFromQuaternion(q);
sim->setGravity(b3MakeVector3(0, 0, -9.8));
sim->setContactBreakingThreshold(0.001);
if (useKitchen)
{
b3RobotSimulatorLoadFileResults res;
sim->loadSDF("kitchens/1.sdf", res);
}
else
{
sim->loadURDF("plane_with_collision_audio.urdf");
}
int handUid = -1;
b3RobotSimulatorLoadFileResults mjcfResults;
const char* mjcfFileName = "MPL/mpl2.xml";
if (sim->loadMJCF(mjcfFileName, mjcfResults))
{
printf("mjcfResults = %d\n", mjcfResults.m_uniqueObjectIds.size());
if (mjcfResults.m_uniqueObjectIds.size() == 1)
{
handUid = mjcfResults.m_uniqueObjectIds[0];
}
}
if (handUid < 0)
{
printf("Cannot load MJCF file %s\n", mjcfFileName);
}
#ifdef TOUCH
b3Vector3 handPos = b3MakeVector3(-0.10, -0.03, 0.02);
b3Vector3 rollPitchYaw = b3MakeVector3(0.5 * B3_PI, 0, 1.25 * B3_PI); //-B3_PI/2,0,B3_PI/2);
handOrn = sim->getQuaternionFromEuler(rollPitchYaw);
#else
b3Quaternion handOrn = sim->getQuaternionFromEuler(b3MakeVector3(3.14, -3.14 / 2, 0));
b3Vector3 handPos = b3MakeVector3(-0.05, 0, 0.02);
#endif
b3Vector3 handStartPosWorld = b3MakeVector3(0.500000, 0.300006, 0.900000);
b3Quaternion handStartOrnWorld = b3Quaternion ::getIdentity();
b3JointInfo jointInfo;
jointInfo.m_jointType = eFixedType;
printf("handStartOrnWorld=%f,%f,%f,%f\n", handStartOrnWorld[0], handStartOrnWorld[1], handStartOrnWorld[2], handStartOrnWorld[3]);
jointInfo.m_childFrame[0] = handStartPosWorld[0];
jointInfo.m_childFrame[1] = handStartPosWorld[1];
jointInfo.m_childFrame[2] = handStartPosWorld[2];
jointInfo.m_childFrame[3] = handStartOrnWorld[0];
jointInfo.m_childFrame[4] = handStartOrnWorld[1];
jointInfo.m_childFrame[5] = handStartOrnWorld[2];
jointInfo.m_childFrame[6] = handStartOrnWorld[3];
jointInfo.m_parentFrame[0] = handPos[0];
jointInfo.m_parentFrame[1] = handPos[1];
jointInfo.m_parentFrame[2] = handPos[2];
jointInfo.m_parentFrame[3] = handOrn[0];
jointInfo.m_parentFrame[4] = handOrn[1];
jointInfo.m_parentFrame[5] = handOrn[2];
jointInfo.m_parentFrame[6] = handOrn[3];
sim->resetBasePositionAndOrientation(handUid, handStartPosWorld, handStartOrnWorld);
int handConstraintId = sim->createConstraint(handUid, -1, -1, -1, &jointInfo);
double maxFingerForce = 10;
double maxArmForce = 1000;
for (int j = 0; j < sim->getNumJoints(handUid); j++)
{
b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD);
controlArgs.m_maxTorqueValue = maxFingerForce;
controlArgs.m_kp = 0.1;
controlArgs.m_kd = 1;
controlArgs.m_targetPosition = 0;
controlArgs.m_targetVelocity = 0;
sim->setJointMotorControl(handUid, j, controlArgs);
}
sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.300000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107)));
sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.200000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107)));
sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.100000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107)));
sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.000000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107)));
sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.900000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107)));
sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.800000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107)));
sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.700000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107)));
sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.600000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107)));
sim->loadURDF("table/table.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.000000, -0.200000, 0.000000), b3Quaternion(0.000000, 0.000000, 0.707107, 0.707107)));
sim->loadURDF("cube_small.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.950000, -0.100000, 0.700000), b3Quaternion(0.000000, 0.000000, 0.707107, 0.707107)));
sim->loadURDF("sphere_small.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.850000, -0.400000, 0.700000), b3Quaternion(0.000000, 0.000000, 0.707107, 0.707107)));
b3Clock clock;
double startTime = clock.getTimeInSeconds();
double simWallClockSeconds = 20.;
int vidLogId = -1;
int minitaurLogId = -1;
int rotateCamera = 0;
serial::Serial my_serial;
try
{
// port, baudrate, timeout in milliseconds
my_serial.setBaudrate(baud);
my_serial.setPort(port);
my_serial.setBytesize(serial::sevenbits);
my_serial.setParity(serial::parity_odd);
my_serial.setStopbits(serial::stopbits_two);
my_serial.setTimeout(serial::Timeout::simpleTimeout(0.01));
my_serial.open();
}
catch (...)
{
printf("Cannot open port, use --port=PORTNAME\n");
exit(0);
}
if (!my_serial.isOpen())
{
printf("Cannot open serial port\n");
return -1;
}
my_serial.flush();
while (sim->canSubmitCommand())
{
clock.usleep(1);
b3VREventsData vrEvents;
sim->getVREvents(&vrEvents, deviceTypeFilter);
//instead of iterating over all vr events, we just take the most up-to-date one
if (vrEvents.m_numControllerEvents)
{
int i = vrEvents.m_numControllerEvents - 1;
b3VRControllerEvent& e = vrEvents.m_controllerEvents[i];
// printf("e.pos=%f,%f,%f\n",e.m_pos[0],e.m_pos[1],e.m_pos[2]);
b3JointInfo changeConstraintInfo;
changeConstraintInfo.m_flags = 0;
changeConstraintInfo.m_jointMaxForce = maxArmForce;
changeConstraintInfo.m_flags |= eJointChangeMaxForce;
changeConstraintInfo.m_childFrame[0] = e.m_pos[0];
changeConstraintInfo.m_childFrame[1] = e.m_pos[1];
changeConstraintInfo.m_childFrame[2] = e.m_pos[2];
changeConstraintInfo.m_flags |= eJointChangeChildFramePosition;
changeConstraintInfo.m_childFrame[3] = e.m_orn[0];
changeConstraintInfo.m_childFrame[4] = e.m_orn[1];
changeConstraintInfo.m_childFrame[5] = e.m_orn[2];
changeConstraintInfo.m_childFrame[6] = e.m_orn[3];
changeConstraintInfo.m_flags |= eJointChangeChildFrameOrientation;
sim->changeConstraint(handConstraintId, &changeConstraintInfo);
}
//read the serial output from the hand, extract into parts
std::string result;
try
{
result = my_serial.readline();
}
catch (...)
{
}
if (result.length())
{
my_serial.flush();
int res = result.find("\n");
while (res < 0)
{
result += my_serial.readline();
res = result.find("\n");
}
btAlignedObjectArray<std::string> pieces;
btAlignedObjectArray<std::string> separators;
separators.push_back(",");
urdfStringSplit(pieces, result, separators);
//printf("serial: %s\n", result.c_str());
if (pieces.size() == 6)
{
double pinkTarget = 0;
double middleTarget = 0;
double indexTarget = 0;
double thumbTarget = 0;
{
int pink = atoi(pieces[1].c_str());
int middle = atoi(pieces[2].c_str());
int index = atoi(pieces[3].c_str());
int thumb = atoi(pieces[4].c_str());
pinkTarget = convertSensor(pink, 250, 400);
middleTarget = convertSensor(middle, 250, 400);
indexTarget = convertSensor(index, 250, 400);
thumbTarget = convertSensor(thumb, 250, 400);
}
//printf("pink = %d, middle=%d, index=%d, thumb=%d\n", pink,middle,index,thumb);
setJointMotorPositionControl(sim, handUid, 5, 1.3);
setJointMotorPositionControl(sim, handUid, 7, thumbTarget);
setJointMotorPositionControl(sim, handUid, 9, thumbTarget);
setJointMotorPositionControl(sim, handUid, 11, thumbTarget);
setJointMotorPositionControl(sim, handUid, 15, indexTarget);
setJointMotorPositionControl(sim, handUid, 17, indexTarget);
setJointMotorPositionControl(sim, handUid, 19, indexTarget);
setJointMotorPositionControl(sim, handUid, 22, middleTarget);
setJointMotorPositionControl(sim, handUid, 24, middleTarget);
setJointMotorPositionControl(sim, handUid, 27, middleTarget);
double ringTarget = 0.5f * (pinkTarget + middleTarget);
setJointMotorPositionControl(sim, handUid, 30, ringTarget);
setJointMotorPositionControl(sim, handUid, 32, ringTarget);
setJointMotorPositionControl(sim, handUid, 34, ringTarget);
setJointMotorPositionControl(sim, handUid, 38, pinkTarget);
setJointMotorPositionControl(sim, handUid, 40, pinkTarget);
setJointMotorPositionControl(sim, handUid, 42, pinkTarget);
}
}
b3KeyboardEventsData keyEvents;
sim->getKeyboardEvents(&keyEvents);
//sim->stepSimulation();
if (rotateCamera)
{
static double yaw = 0;
double distance = 1;
yaw += 0.1;
b3Vector3 basePos;
b3Quaternion baseOrn;
// sim->getBasePositionAndOrientation(minitaurUid,basePos,baseOrn);
// sim->resetDebugVisualizerCamera(distance,yaw,20,basePos);
}
//b3Clock::usleep(1000.*1000.*fixedTimeStep);
}
printf("serial.close()\n");
my_serial.close();
printf("sim->disconnect\n");
sim->disconnect();
printf("delete sim\n");
delete sim;
printf("exit\n");
}

197
Engine/lib/bullet/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp Normal file
View file

@ -0,0 +1,197 @@
#include "b3RobotSimulatorClientAPI.h"
#include "../SharedMemory/PhysicsClientC_API.h"
#include "../SharedMemory/b3RobotSimulatorClientAPI_InternalData.h"
#ifdef BT_ENABLE_ENET
#include "../SharedMemory/PhysicsClientUDP_C_API.h"
#endif //PHYSICS_UDP
#ifdef BT_ENABLE_CLSOCKET
#include "../SharedMemory/PhysicsClientTCP_C_API.h"
#endif //PHYSICS_TCP
#include "../SharedMemory/PhysicsDirectC_API.h"
#include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
#include "../SharedMemory/SharedMemoryPublic.h"
#include "Bullet3Common/b3Logging.h"
#ifdef BT_ENABLE_GRPC
#include "../SharedMemory/PhysicsClientGRPC_C_API.h"
#endif
b3RobotSimulatorClientAPI::b3RobotSimulatorClientAPI()
{
}
b3RobotSimulatorClientAPI::~b3RobotSimulatorClientAPI()
{
}
void b3RobotSimulatorClientAPI::renderScene()
{
if (!isConnected())
{
b3Warning("Not connected");
return;
}
if (m_data->m_guiHelper)
{
b3InProcessRenderSceneInternal(m_data->m_physicsClientHandle);
}
}
void b3RobotSimulatorClientAPI::debugDraw(int debugDrawMode)
{
if (!isConnected())
{
b3Warning("Not connected");
return;
}
if (m_data->m_guiHelper)
{
b3InProcessDebugDrawInternal(m_data->m_physicsClientHandle, debugDrawMode);
}
}
bool b3RobotSimulatorClientAPI::mouseMoveCallback(float x, float y)
{
if (!isConnected())
{
b3Warning("Not connected");
return false;
}
if (m_data->m_guiHelper)
{
return b3InProcessMouseMoveCallback(m_data->m_physicsClientHandle, x, y) != 0;
}
return false;
}
bool b3RobotSimulatorClientAPI::mouseButtonCallback(int button, int state, float x, float y)
{
if (!isConnected())
{
b3Warning("Not connected");
return false;
}
if (m_data->m_guiHelper)
{
return b3InProcessMouseButtonCallback(m_data->m_physicsClientHandle, button, state, x, y) != 0;
}
return false;
}
bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, int portOrKey)
{
if (m_data->m_physicsClientHandle)
{
b3Warning("Already connected, disconnect first.");
return false;
}
b3PhysicsClientHandle sm = 0;
int udpPort = 1234;
int tcpPort = 6667;
int key = SHARED_MEMORY_KEY;
switch (mode)
{
case eCONNECT_EXISTING_EXAMPLE_BROWSER:
{
sm = b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(m_data->m_guiHelper);
break;
}
case eCONNECT_GUI:
{
int argc = 0;
char* argv[1] = {0};
#ifdef __APPLE__
sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
#else
sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
#endif
break;
}
case eCONNECT_GUI_SERVER:
{
int argc = 0;
char* argv[1] = {0};
#ifdef __APPLE__
sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
#else
sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
#endif
break;
}
case eCONNECT_DIRECT:
{
sm = b3ConnectPhysicsDirect();
break;
}
case eCONNECT_SHARED_MEMORY:
{
if (portOrKey >= 0)
{
key = portOrKey;
}
sm = b3ConnectSharedMemory(key);
break;
}
case eCONNECT_UDP:
{
if (portOrKey >= 0)
{
udpPort = portOrKey;
}
#ifdef BT_ENABLE_ENET
sm = b3ConnectPhysicsUDP(hostName.c_str(), udpPort);
#else
b3Warning("UDP is not enabled in this build");
#endif //BT_ENABLE_ENET
break;
}
case eCONNECT_TCP:
{
if (portOrKey >= 0)
{
tcpPort = portOrKey;
}
#ifdef BT_ENABLE_CLSOCKET
sm = b3ConnectPhysicsTCP(hostName.c_str(), tcpPort);
#else
b3Warning("TCP is not enabled in this pybullet build");
#endif //BT_ENABLE_CLSOCKET
break;
}
case eCONNECT_GRPC:
{
#ifdef BT_ENABLE_GRPC
sm = b3ConnectPhysicsGRPC(hostName.c_str(), tcpPort);
#else
b3Warning("GRPC is not enabled in this pybullet build");
#endif
break;
}
default:
{
b3Warning("connectPhysicsServer unexpected argument");
}
};
if (sm)
{
m_data->m_physicsClientHandle = sm;
if (!b3CanSubmitCommand(m_data->m_physicsClientHandle))
{
disconnect();
return false;
}
return true;
}
return false;
}

27
Engine/lib/bullet/examples/RobotSimulator/b3RobotSimulatorClientAPI.h Normal file
View file

@ -0,0 +1,27 @@
#ifndef B3_ROBOT_SIMULATOR_CLIENT_API_GUI_H
#define B3_ROBOT_SIMULATOR_CLIENT_API_GUI_H
#include "../SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h"
///The b3RobotSimulatorClientAPI_GUI is pretty much the C++ version of pybullet
///as documented in the pybullet Quickstart Guide
///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
class b3RobotSimulatorClientAPI : public b3RobotSimulatorClientAPI_NoGUI
{
public:
b3RobotSimulatorClientAPI();
virtual ~b3RobotSimulatorClientAPI();
virtual bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1);
virtual void renderScene();
virtual void debugDraw(int debugDrawMode);
virtual bool mouseMoveCallback(float x, float y);
virtual bool mouseButtonCallback(int button, int state, float x, float y);
};
#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H

299
Engine/lib/bullet/examples/RobotSimulator/premake4.lua Normal file
View file

@ -0,0 +1,299 @@
project ("App_RobotSimulator")
language "C++"
kind "ConsoleApp"
includedirs {"../../src", "../../examples",
"../../examples/ThirdPartyLibs"}
defines {"B3_USE_ROBOTSIM_GUI", "PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
if _OPTIONS["enable_grpc"] then
initGRPC()
files {
"../../examples/SharedMemory/PhysicsClientGRPC.cpp",
"../../examples/SharedMemory/PhysicsClientGRPC.h",
"../../examples/SharedMemory/PhysicsClientGRPC_C_API.cpp",
"../../examples/SharedMemory/PhysicsClientGRPC_C_API.h",
}
end
links{"BulletRobotics", "BulletExampleBrowserLib", "gwen", "OpenGL_Window","BulletFileLoader","BulletWorldImporter","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"}
initOpenGL()
initGlew()
includedirs {
".",
"../../src",
"../../examples/SharedMemory",
"../ThirdPartyLibs",
}
if os.is("MacOSX") then
links{"Cocoa.framework"}
end
if not _OPTIONS["no-enet"] then
includedirs {"../../examples/ThirdPartyLibs/enet/include"}
if os.is("Windows") then
-- targetextension {"dylib"}
defines { "WIN32" }
links {"Ws2_32","Winmm"}
end
if os.is("Linux") then
end
if os.is("MacOSX") then
end
links {"enet"}
files {
"../../examples/SharedMemory/PhysicsClientUDP.cpp",
"../../examples/SharedMemory/PhysicsClientUDP.h",
"../../examples/SharedMemory/PhysicsClientUDP_C_API.cpp",
"../../examples/SharedMemory/PhysicsClientUDP_C_API.h",
}
defines {"BT_ENABLE_ENET"}
end
if not _OPTIONS["no-clsocket"] then
includedirs {"../../examples/ThirdPartyLibs/clsocket/src"}
if os.is("Windows") then
defines { "WIN32" }
links {"Ws2_32","Winmm"}
end
if os.is("Linux") then
defines {"_LINUX"}
end
if os.is("MacOSX") then
defines {"_DARWIN"}
end
links {"clsocket"}
files {
"../../examples/SharedMemory/PhysicsClientTCP.cpp",
"../../examples/SharedMemory/PhysicsClientTCP.h",
"../../examples/SharedMemory/PhysicsClientTCP_C_API.cpp",
"../../examples/SharedMemory/PhysicsClientTCP_C_API.h",
}
defines {"BT_ENABLE_CLSOCKET"}
end
if _OPTIONS["audio"] then
files {
"../TinyAudio/b3ADSR.cpp",
"../TinyAudio/b3AudioListener.cpp",
"../TinyAudio/b3ReadWavFile.cpp",
"../TinyAudio/b3SoundEngine.cpp",
"../TinyAudio/b3SoundSource.cpp",
"../TinyAudio/b3WriteWavFile.cpp",
"../TinyAudio/RtAudio.cpp",
}
defines {"B3_ENABLE_TINY_AUDIO"}
if _OPTIONS["serial"] then
defines{"B3_ENABLE_SERIAL"}
includedirs {"../../examples/ThirdPartyLibs/serial/include"}
links {"serial"}
end
if os.is("Windows") then
links {"winmm","Wsock32","dsound"}
defines {"WIN32","__WINDOWS_MM__","__WINDOWS_DS__"}
end
if os.is("Linux") then initX11()
defines {"__OS_LINUX__","__LINUX_ALSA__"}
links {"asound","pthread"}
end
if os.is("MacOSX") then
links{"Cocoa.framework"}
links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"}
defines {"__OS_MACOSX__","__MACOSX_CORE__"}
end
end
files {
"RobotSimulatorMain.cpp",
"b3RobotSimulatorClientAPI.cpp",
"b3RobotSimulatorClientAPI.h",
"MinitaurSetup.cpp",
"MinitaurSetup.h",
"../../examples/ExampleBrowser/InProcessExampleBrowser.cpp",
"../../examples/SharedMemory/PhysicsServerExample.cpp",
"../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp",
"../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp",
}
if (_OPTIONS["enable_static_vr_plugin"]) then
files {"../../examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp"}
end
if os.is("Linux") then
initX11()
end
if _OPTIONS["serial"] then
project ("App_VRGloveHandSimulator")
language "C++"
kind "ConsoleApp"
includedirs {"../../src", "../../examples",
"../../examples/ThirdPartyLibs"}
defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
hasCL = findOpenCL("clew")
links{"BulletRobotics", "BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletFileLoader","BulletWorldImporter","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK","Bullet3Common"}
initOpenGL()
initGlew()
includedirs {
".",
"../../src",
"../ThirdPartyLibs",
}
if os.is("MacOSX") then
links{"Cocoa.framework"}
end
if (hasCL) then
links {
"Bullet3OpenCL_clew",
"Bullet3Dynamics",
"Bullet3Collision",
"Bullet3Geometry",
"Bullet3Common",
}
end
if _OPTIONS["audio"] then
files {
"../TinyAudio/b3ADSR.cpp",
"../TinyAudio/b3AudioListener.cpp",
"../TinyAudio/b3ReadWavFile.cpp",
"../TinyAudio/b3SoundEngine.cpp",
"../TinyAudio/b3SoundSource.cpp",
"../TinyAudio/b3WriteWavFile.cpp",
"../TinyAudio/RtAudio.cpp",
}
defines {"B3_ENABLE_TINY_AUDIO"}
defines{"B3_ENABLE_SERIAL"}
includedirs {"../../examples/ThirdPartyLibs/serial/include"}
links {"serial"}
if os.is("Windows") then
links {"winmm","Wsock32","dsound"}
defines {"WIN32","__WINDOWS_MM__","__WINDOWS_DS__"}
end
if os.is("Linux") then initX11()
defines {"__OS_LINUX__","__LINUX_ALSA__"}
links {"asound","pthread"}
end
if os.is("MacOSX") then
links{"Cocoa.framework"}
links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"}
defines {"__OS_MACOSX__","__MACOSX_CORE__"}
end
end
files {
"VRGloveSimulatorMain.cpp",
"b3RobotSimulatorClientAPI.cpp",
"b3RobotSimulatorClientAPI.h",
}
if (_OPTIONS["enable_static_vr_plugin"]) then
files {"../../examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp"}
end
if os.is("Linux") then
initX11()
end
end
project ("App_HelloBulletRobotics")
language "C++"
kind "ConsoleApp"
links{"BulletRobotics","BulletFileLoader","BulletWorldImporter","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"}
includedirs {
".",
"../../src",
"../../examples/SharedMemory",
"../ThirdPartyLibs",
}
if not _OPTIONS["no-enet"] then
includedirs {"../../examples/ThirdPartyLibs/enet/include"}
if os.is("Windows") then
defines { "WIN32" }
links {"Ws2_32","Winmm"}
end
if os.is("Linux") then
end
if os.is("MacOSX") then
end
links {"enet"}
defines {"BT_ENABLE_ENET"}
end
if not _OPTIONS["no-clsocket"] then
includedirs {"../../examples/ThirdPartyLibs/clsocket/src"}
if os.is("Windows") then
defines { "WIN32" }
links {"Ws2_32","Winmm"}
end
if os.is("Linux") then
defines {"_LINUX"}
end
if os.is("MacOSX") then
defines {"_DARWIN"}
end
links {"clsocket"}
defines {"BT_ENABLE_CLSOCKET"}
end
if os.is("MacOSX") then
links{"Cocoa.framework"}
end
if os.is("Linux") then initX11()
links {"pthread"}
end
files {
"HelloBulletRobotics.cpp"
}