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

Code-style consistency improvement:

Browse Source 
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files.
make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type.
This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
2018-09-23 14:17:31 -07:00
parent b73b05e9fb
commit ab8f16961e
1773 changed files with 1081087 additions and 474249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
examples/RobotSimulator/HelloBulletRobotics.cpp
View File

@@ -7,45 +7,41 @@ int main(int argc, char* argv[])
bool isConnected = sim->connect(eCONNECT_SHARED_MEMORY);
if (!isConnected)
{
printf("Using Direct mode\n");
isConnected = sim->connect(eCONNECT_DIRECT);
} else
}
else
{
printf("Using shared memory\n");
}
//remove all existing objects (if any)
sim->resetSimulation();
sim->setGravity(btVector3(0,0,-9.8));
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);
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->getBasePositionAndOrientation(r2d2Uid, basePos, baseOrn);
printf("r2d2 basePosition = [%f,%f,%f]\n", basePos[0], basePos[1], basePos[2]);
sim->stepSimulation();
}
delete sim;

340
examples/RobotSimulator/MinitaurSetup.cpp
View File

@@ -8,15 +8,13 @@ struct MinitaurSetupInternalData
int m_quadrupedUniqueId;
MinitaurSetupInternalData()
:m_quadrupedUniqueId(-1)
: m_quadrupedUniqueId(-1)
{
}
b3HashMap<b3HashString, int> m_jointNameToId;
};
MinitaurSetup::MinitaurSetup()
{
m_data = new MinitaurSetupInternalData();
@@ -34,7 +32,7 @@ void MinitaurSetup::setDesiredMotorAngle(class b3RobotSimulatorClientAPI_NoGUI*
controlArgs.m_kd = kd;
controlArgs.m_kp = kp;
controlArgs.m_targetPosition = desiredAngle;
sim->setJointMotorControl(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId[motorName],controlArgs);
sim->setJointMotorControl(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[motorName], controlArgs);
}
//pick exactly 1 configuration of the following
@@ -42,201 +40,210 @@ void MinitaurSetup::setDesiredMotorAngle(class b3RobotSimulatorClientAPI_NoGUI*
//#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* 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* 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* 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 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);
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* 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* 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);
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* 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* 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);
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++)
for (int i = 0; i < numJoints; i++)
{
b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_maxTorqueValue = 0;
sim->setJointMotorControl(m_data->m_quadrupedUniqueId,i,controlArgs);
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};
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);
//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_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);
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];
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);
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);
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);
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);
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);
@@ -244,34 +251,32 @@ void MinitaurSetup::resetPose(class b3RobotSimulatorClientAPI_NoGUI* sim)
controlArgs.m_kd = 1;
controlArgs.m_kp = 0;
controlArgs.m_targetPosition = 0;
for (int i=0;i<4;i++)
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);
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);
m_data->m_quadrupedUniqueId = sim->loadURDF(minitaurURDF, args);
int numJoints = sim->getNumJoints(m_data->m_quadrupedUniqueId);
for (int i=0;i<numJoints;i++)
for (int i = 0; i < numJoints; i++)
{
b3JointInfo jointInfo;
sim->getJointInfo(m_data->m_quadrupedUniqueId,i,&jointInfo);
sim->getJointInfo(m_data->m_quadrupedUniqueId, i, &jointInfo);
if (jointInfo.m_jointName[0])
{
m_data->m_jointNameToId.insert(jointInfo.m_jointName,i);
m_data->m_jointNameToId.insert(jointInfo.m_jointName, i);
}
}
@@ -279,4 +284,3 @@ int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI_NoGUI* sim, con
return m_data->m_quadrupedUniqueId;
}

8
examples/RobotSimulator/MinitaurSetup.h
View File

@@ -12,10 +12,8 @@ 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);
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
#endif //MINITAUR_SIMULATION_SETUP_H

66
examples/RobotSimulator/RobotSimulatorMain.cpp
View File

@@ -1,21 +1,18 @@
#ifdef B3_USE_ROBOTSIM_GUI
#include "b3RobotSimulatorClientAPI.h"
#include "b3RobotSimulatorClientAPI.h"
#else
#include "b3RobotSimulatorClientAPI_NoGUI.h"
#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));
#define ASSERT_EQ(a, b) assert((a) == (b));
#include "MinitaurSetup.h"
int main(int argc, char* argv[])
{
#ifdef B3_USE_ROBOTSIM_GUI
@@ -32,20 +29,20 @@ int main(int argc, char* argv[])
}
//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->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.;
btScalar fixedTimeStep = 1. / 240.;
sim->setTimeStep(fixedTimeStep);
btQuaternion q = sim->getQuaternionFromEuler(btVector3(0.1,0.2,0.3));
btQuaternion q = sim->getQuaternionFromEuler(btVector3(0.1, 0.2, 0.3));
btVector3 rpy;
rpy = sim->getEulerFromQuaternion(q);
sim->setGravity(btVector3(0,0,-9.8));
sim->setGravity(btVector3(0, 0, -9.8));
//int blockId = sim->loadURDF("cube.urdf");
//b3BodyInfo bodyInfo;
@@ -54,9 +51,8 @@ int main(int argc, char* argv[])
sim->loadURDF("plane.urdf");
MinitaurSetup minitaur;
int minitaurUid = minitaur.setupMinitaur(sim, btVector3(0,0,.3));
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);
@@ -64,7 +60,7 @@ int main(int argc, char* argv[])
//b3RobotSimulatorLoadFileResults sdfResults;
//if (!sim->loadSDF("two_cubes.sdf",sdfResults))
//{
// b3Warning("Can't load SDF!\n");
// b3Warning("Can't load SDF!\n");
//}
b3Clock clock;
@@ -87,48 +83,46 @@ int main(int argc, char* argv[])
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++)
for (int i = 0; i < keyEvents.m_numKeyboardEvents; i++)
{
b3KeyboardEvent& e = keyEvents.m_keyboardEvents[i];
if (e.m_keyCode=='0')
if (e.m_keyCode == '0')
{
if ( e.m_keyState&eButtonTriggered)
if (e.m_keyState & eButtonTriggered)
{
if (vidLogId < 0)
{
vidLogId = sim->startStateLogging(STATE_LOGGING_VIDEO_MP4,"video.mp4");
}
vidLogId = sim->startStateLogging(STATE_LOGGING_VIDEO_MP4, "video.mp4");
}
else
{
sim->stopStateLogging(vidLogId);
vidLogId=-1;
vidLogId = -1;
}
}
}
if (e.m_keyCode=='m')
if (e.m_keyCode == 'm')
{
if ( minitaurLogId<0 && e.m_keyState&eButtonTriggered)
if (minitaurLogId < 0 && e.m_keyState & eButtonTriggered)
{
minitaurLogId = sim->startStateLogging(STATE_LOGGING_MINITAUR,"simlog.bin");
minitaurLogId = sim->startStateLogging(STATE_LOGGING_MINITAUR, "simlog.bin");
}
if (minitaurLogId>=0 && e.m_keyState&eButtonReleased)
if (minitaurLogId >= 0 && e.m_keyState & eButtonReleased)
{
sim->stopStateLogging(minitaurLogId);
minitaurLogId=-1;
minitaurLogId = -1;
}
}
if (e.m_keyCode == 'r' && e.m_keyState&eButtonTriggered)
if (e.m_keyCode == 'r' && e.m_keyState & eButtonTriggered)
{
rotateCamera = 1-rotateCamera;
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);
}
}
@@ -136,25 +130,23 @@ int main(int argc, char* argv[])
if (rotateCamera)
{
static double yaw=0;
static double yaw = 0;
double distance = 1;
yaw+=0.1;
yaw += 0.1;
btVector3 basePos;
btQuaternion baseOrn;
sim->getBasePositionAndOrientation(minitaurUid,basePos,baseOrn);
sim->resetDebugVisualizerCamera(distance,-20, yaw,basePos);
sim->getBasePositionAndOrientation(minitaurUid, basePos, baseOrn);
sim->resetDebugVisualizerCamera(distance, -20, yaw, basePos);
}
b3Clock::usleep(1000.*1000.*fixedTimeStep);
b3Clock::usleep(1000. * 1000. * fixedTimeStep);
}
printf("sim->disconnect\n");
sim->disconnect();
printf("delete sim\n");
delete sim;
printf("exit\n");
}

198
examples/RobotSimulator/VRGloveSimulatorMain.cpp
View File

@@ -12,9 +12,9 @@
double convertSensor(int inputV, int minV, int maxV)
{
b3Clamp(inputV,minV,maxV);
double outVal =(double)inputV;
double b = (outVal-(double)minV)/float(maxV-minV);
b3Clamp(inputV, minV, maxV);
double outVal = (double)inputV;
double b = (outVal - (double)minV) / float(maxV - minV);
return (b);
}
@@ -24,44 +24,44 @@ void setJointMotorPositionControl(b3RobotSimulatorClientAPI* sim, int obUid, int
controlArgs.m_maxTorqueValue = 50.;
controlArgs.m_targetPosition = targetPosition;
controlArgs.m_targetVelocity = 0;
sim->setJointMotorControl(obUid,linkIndex,controlArgs);
sim->setJointMotorControl(obUid, linkIndex, controlArgs);
}
int main(int argc, char* argv[])
{
b3CommandLineArgs args(argc,argv);
std::string port="COM9";
args.GetCmdLineArgument("port",port);
b3CommandLineArgs args(argc, argv);
std::string port = "COM9";
args.GetCmdLineArgument("port", port);
int baud = 115200;
args.GetCmdLineArgument("speed",baud);
args.GetCmdLineArgument("speed", baud);
std::string mode = "SHARED_MEMORY";
args.GetCmdLineArgument("mode",mode);
args.GetCmdLineArgument("mode", mode);
int disableGui = 0;
args.GetCmdLineArgument("disableGui",disableGui);
args.GetCmdLineArgument("disableGui", disableGui);
int disableShadows = 0;
args.GetCmdLineArgument("disableShadows",disableShadows);
args.GetCmdLineArgument("disableShadows", disableShadows);
int useKitchen = 0;
args.GetCmdLineArgument("useKitchen",useKitchen);
int deviceTypeFilter = VR_DEVICE_GENERIC_TRACKER;
args.GetCmdLineArgument("deviceTypeFilter",deviceTypeFilter);
args.GetCmdLineArgument("useKitchen", useKitchen);
printf("port=%s, speed=%d, connection mode=%s\n", port.c_str(), baud,mode.c_str());
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")
if (mode == "GUI")
{
sim->connect(eCONNECT_GUI);
} else
}
else
{
if (mode=="DIRECT")
if (mode == "DIRECT")
{
sim->connect(eCONNECT_DIRECT);
} else
}
else
{
sim->connect(eCONNECT_SHARED_MEMORY);
}
@@ -73,73 +73,72 @@ int main(int argc, char* argv[])
if (disableGui)
{
sim->configureDebugVisualizer( COV_ENABLE_GUI, 0);
sim->configureDebugVisualizer(COV_ENABLE_GUI, 0);
}
if (disableShadows)
{
sim->configureDebugVisualizer( COV_ENABLE_SHADOWS, 0);
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.;
b3Scalar fixedTimeStep = 1. / 240.;
sim->setTimeStep(fixedTimeStep);
b3Quaternion q = sim->getQuaternionFromEuler(b3MakeVector3(0.1,0.2,0.3));
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->setGravity(b3MakeVector3(0, 0, -9.8));
sim->setContactBreakingThreshold(0.001);
if (useKitchen)
{
b3RobotSimulatorLoadFileResults res;
sim->loadSDF("kitchens/1.sdf",res);
} else
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))
if (sim->loadMJCF(mjcfFileName, mjcfResults))
{
printf("mjcfResults = %d\n", mjcfResults.m_uniqueObjectIds.size());
if (mjcfResults.m_uniqueObjectIds.size()==1)
if (mjcfResults.m_uniqueObjectIds.size() == 1)
{
handUid = mjcfResults.m_uniqueObjectIds[0];
}
}
if (handUid<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);
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);
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);
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]);
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];
@@ -156,34 +155,33 @@ int main(int argc, char* argv[])
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);
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++)
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_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->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)));
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.;
@@ -203,7 +201,8 @@ int main(int argc, char* argv[])
my_serial.setStopbits(serial::stopbits_two);
my_serial.setTimeout(serial::Timeout::simpleTimeout(0.01));
my_serial.open();
} catch(...)
}
catch (...)
{
printf("Cannot open port, use --port=PORTNAME\n");
exit(0);
@@ -215,10 +214,8 @@ int main(int argc, char* argv[])
return -1;
}
my_serial.flush();
while (sim->canSubmitCommand())
{
clock.usleep(1);
@@ -231,10 +228,10 @@ int main(int argc, char* argv[])
{
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]);
// 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_jointMaxForce = maxArmForce;
changeConstraintInfo.m_flags |= eJointChangeMaxForce;
changeConstraintInfo.m_childFrame[0] = e.m_pos[0];
@@ -256,15 +253,15 @@ int main(int argc, char* argv[])
try
{
result = my_serial.readline();
} catch(...)
}
catch (...)
{
}
if (result.length())
{
my_serial.flush();
int res = result.find("\n");
while (res<0)
while (res < 0)
{
result += my_serial.readline();
res = result.find("\n");
@@ -272,67 +269,67 @@ int main(int argc, char* argv[])
btAlignedObjectArray<std::string> pieces;
btAlignedObjectArray<std::string> separators;
separators.push_back(",");
urdfStringSplit( pieces, result, separators);
urdfStringSplit(pieces, result, separators);
//printf("serial: %s\n", result.c_str());
if (pieces.size()==6)
if (pieces.size() == 6)
{
double pinkTarget = 0;
double middleTarget = 0;
double indexTarget = 0;
double thumbTarget = 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);
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, 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, 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);
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);
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);
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;
static double yaw = 0;
double distance = 1;
yaw+=0.1;
yaw += 0.1;
b3Vector3 basePos;
b3Quaternion baseOrn;
// sim->getBasePositionAndOrientation(minitaurUid,basePos,baseOrn);
// sim->resetDebugVisualizerCamera(distance,yaw,20,basePos);
// sim->getBasePositionAndOrientation(minitaurUid,basePos,baseOrn);
// sim->resetDebugVisualizerCamera(distance,yaw,20,basePos);
}
//b3Clock::usleep(1000.*1000.*fixedTimeStep);
}
@@ -342,10 +339,9 @@ int main(int argc, char* argv[])
printf("sim->disconnect\n");
sim->disconnect();
printf("delete sim\n");
delete sim;
printf("exit\n");
}

43
examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp
View File

@@ -23,15 +23,12 @@
b3RobotSimulatorClientAPI::b3RobotSimulatorClientAPI()
{
}
b3RobotSimulatorClientAPI::~b3RobotSimulatorClientAPI()
{
}
void b3RobotSimulatorClientAPI::renderScene()
{
if (!isConnected())
@@ -54,11 +51,11 @@ void b3RobotSimulatorClientAPI::debugDraw(int debugDrawMode)
}
if (m_data->m_guiHelper)
{
b3InProcessDebugDrawInternal(m_data->m_physicsClientHandle,debugDrawMode);
b3InProcessDebugDrawInternal(m_data->m_physicsClientHandle, debugDrawMode);
}
}
bool b3RobotSimulatorClientAPI::mouseMoveCallback(float x,float y)
bool b3RobotSimulatorClientAPI::mouseMoveCallback(float x, float y)
{
if (!isConnected())
{
@@ -67,11 +64,11 @@ bool b3RobotSimulatorClientAPI::mouseMoveCallback(float x,float y)
}
if (m_data->m_guiHelper)
{
return b3InProcessMouseMoveCallback(m_data->m_physicsClientHandle, x,y)!=0;
return b3InProcessMouseMoveCallback(m_data->m_physicsClientHandle, x, y) != 0;
}
return false;
}
bool b3RobotSimulatorClientAPI::mouseButtonCallback(int button, int state, float x, float y)
bool b3RobotSimulatorClientAPI::mouseButtonCallback(int button, int state, float x, float y)
{
if (!isConnected())
{
@@ -80,13 +77,11 @@ bool b3RobotSimulatorClientAPI::mouseButtonCallback(int button, int state, float
}
if (m_data->m_guiHelper)
{
return b3InProcessMouseButtonCallback(m_data->m_physicsClientHandle, button,state,x,y)!=0;
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)
@@ -102,13 +97,13 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
switch (mode)
{
case eCONNECT_EXISTING_EXAMPLE_BROWSER:
case eCONNECT_EXISTING_EXAMPLE_BROWSER:
{
sm = b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(m_data->m_guiHelper);
break;
}
case eCONNECT_GUI:
case eCONNECT_GUI:
{
int argc = 0;
char* argv[1] = {0};
@@ -119,7 +114,7 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
#endif
break;
}
case eCONNECT_GUI_SERVER:
case eCONNECT_GUI_SERVER:
{
int argc = 0;
char* argv[1] = {0};
@@ -130,12 +125,12 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
#endif
break;
}
case eCONNECT_DIRECT:
case eCONNECT_DIRECT:
{
sm = b3ConnectPhysicsDirect();
break;
}
case eCONNECT_SHARED_MEMORY:
case eCONNECT_SHARED_MEMORY:
{
if (portOrKey >= 0)
{
@@ -144,7 +139,7 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
sm = b3ConnectSharedMemory(key);
break;
}
case eCONNECT_UDP:
case eCONNECT_UDP:
{
if (portOrKey >= 0)
{
@@ -159,7 +154,7 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
break;
}
case eCONNECT_TCP:
case eCONNECT_TCP:
{
if (portOrKey >= 0)
{
@@ -173,16 +168,16 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
#endif //BT_ENABLE_CLSOCKET
break;
}
case eCONNECT_GRPC:
{
case eCONNECT_GRPC:
{
#ifdef BT_ENABLE_GRPC
sm = b3ConnectPhysicsGRPC(hostName.c_str(), tcpPort);
sm = b3ConnectPhysicsGRPC(hostName.c_str(), tcpPort);
#else
b3Warning("GRPC is not enabled in this pybullet build");
b3Warning("GRPC is not enabled in this pybullet build");
#endif
break;
}
default:
break;
}
default:
{
b3Warning("connectPhysicsServer unexpected argument");
}

13
examples/RobotSimulator/b3RobotSimulatorClientAPI.h
View File

@@ -3,30 +3,25 @@
#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);
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