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

update RobotSimulator/MinitaurSetup to use data/quadruped/minitaur.urdf

Browse Source 
add b3RobotSimulatorClientAPI::getBaseVelocity and resetBaseVelocity
add b3Quaternion::getEulerZYX
This commit is contained in:
Erwin Coumans
2017-03-15 17:09:17 -07:00
parent 4db6fa9e29
commit b7b46b12d3
5 changed files with 427 additions and 381 deletions
Download Patch File Download Diff File Expand all files Collapse all files

98
examples/RobotSimulator/MinitaurSetup.cpp
View File

@@ -49,57 +49,69 @@ void MinitaurSetup::resetPose(class b3RobotSimulatorClientAPI* sim)
sim->setJointMotorControl(m_data->m_quadrupedUniqueId,i,controlArgs);
}
//right front leg
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightR_joint"],1.57);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightR_link"],-2.2);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightL_joint"],-1.57);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],2.2);
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;
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.01; jointInfo.m_parentFrame[2] = 0.2;
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = -0.015; jointInfo.m_childFrame[2] = 0.2;
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightR_link"],
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],&jointInfo);
setDesiredMotorAngle(sim,"motor_front_rightR_joint",1.57);
setDesiredMotorAngle(sim,"motor_front_rightL_joint",-1.57);
//left front leg
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftR_joint"],1.57);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftR_link"],-2.2);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftL_joint"],-1.57);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftL_link"],2.2);
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = -0.01; jointInfo.m_parentFrame[2] = 0.2;
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.015; jointInfo.m_childFrame[2] = 0.2;
//left front leg
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftL_joint"],motorDirs[0] * startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftL_link"],motorDirs[0]*kneeAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftR_joint"],motorDirs[1] * startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftR_link"],motorDirs[1]*kneeAngle);
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.005; jointInfo.m_parentFrame[2] = 0.2;
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.01; jointInfo.m_childFrame[2] = 0.2;
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftR_link"],
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftL_link"],&jointInfo);
setDesiredMotorAngle(sim,"motor_front_leftR_joint", 1.57);
setDesiredMotorAngle(sim,"motor_front_leftL_joint", -1.57);
//right back leg
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_rightR_joint"],1.57);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightR_link"],-2.2);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_rightL_joint"],-1.57);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightL_link"],2.2);
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.01; jointInfo.m_parentFrame[2] = 0.2;
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = -0.015; jointInfo.m_childFrame[2] = 0.2;
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightR_link"],
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightL_link"],&jointInfo);
setDesiredMotorAngle(sim,"motor_back_rightR_joint", 1.57);
setDesiredMotorAngle(sim,"motor_back_rightL_joint", -1.57);
setDesiredMotorAngle(sim,"motor_front_leftL_joint", motorDirs[0] * startAngle);
setDesiredMotorAngle(sim,"motor_front_leftR_joint", motorDirs[1] * startAngle);
//left back leg
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_leftR_joint"],1.57);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftR_link"],-2.2);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_leftL_joint"],-1.57);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftL_link"],2.2);
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = -0.01; jointInfo.m_parentFrame[2] = 0.2;
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.015; jointInfo.m_childFrame[2] = 0.2;
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_leftL_joint"],motorDirs[2] * startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftL_link"],motorDirs[2] * kneeAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_leftR_joint"],motorDirs[3] * startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftR_link"],motorDirs[3] * kneeAngle);
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.005; jointInfo.m_parentFrame[2] = 0.2;
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.01; jointInfo.m_childFrame[2] = 0.2;
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftR_link"],
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftL_link"],&jointInfo);
setDesiredMotorAngle(sim,"motor_back_leftR_joint", 1.57);
setDesiredMotorAngle(sim,"motor_back_leftL_joint", -1.57);
setDesiredMotorAngle(sim,"motor_back_leftL_joint", motorDirs[2] * startAngle);
setDesiredMotorAngle(sim,"motor_back_leftR_joint", motorDirs[3] * startAngle);
//right front leg
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightL_joint"],motorDirs[4] * startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],motorDirs[4] * kneeAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightR_joint"],motorDirs[5]*startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightR_link"],motorDirs[5] * kneeAngle);
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.005; jointInfo.m_parentFrame[2] = 0.2;
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.01; jointInfo.m_childFrame[2] = 0.2;
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightR_link"],
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],&jointInfo);
setDesiredMotorAngle(sim,"motor_front_rightL_joint",motorDirs[4] * startAngle);
setDesiredMotorAngle(sim,"motor_front_rightR_joint",motorDirs[5] * startAngle);
//right back leg
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_rightL_joint"],motorDirs[6] * startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightL_link"],motorDirs[6] * kneeAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_rightR_joint"],motorDirs[7] * startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightR_link"],motorDirs[7] * kneeAngle);
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.005; jointInfo.m_parentFrame[2] = 0.2;
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.01; jointInfo.m_childFrame[2] = 0.2;
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightR_link"],
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightL_link"],&jointInfo);
setDesiredMotorAngle(sim,"motor_back_rightL_joint", motorDirs[6] * startAngle);
setDesiredMotorAngle(sim,"motor_back_rightR_joint", motorDirs[7] * startAngle);
}
int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3Vector3& startPos, const b3Quaternion& startOrn)
@@ -109,7 +121,7 @@ int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3V
args.m_startPosition = startPos;
args.m_startOrientation = startOrn;
m_data->m_quadrupedUniqueId = sim->loadURDF("quadruped/quadruped.urdf",args);
m_data->m_quadrupedUniqueId = sim->loadURDF("quadruped/minitaur.urdf",args);
int numJoints = sim->getNumJoints(m_data->m_quadrupedUniqueId);
for (int i=0;i<numJoints;i++)

36
examples/RobotSimulator/RobotSimulatorMain.cpp
View File

@@ -19,14 +19,19 @@ int main(int argc, char* argv[])
//syncBodies is only needed when connecting to an existing physics server that has already some bodies
sim->syncBodies();
sim->setTimeStep(1./240.);
b3Scalar fixedTimeStep = 1./240.;
sim->setGravity(b3MakeVector3(0,0,-10));
sim->setTimeStep(fixedTimeStep);
int blockId = sim->loadURDF("cube.urdf");
b3BodyInfo bodyInfo;
sim->getBodyInfo(blockId,&bodyInfo);
b3Quaternion q = sim->getQuaternionFromEuler(b3MakeVector3(0.1,0.2,0.3));
b3Vector3 rpy;
rpy = sim->getEulerFromQuaternion(q);
sim->setGravity(b3MakeVector3(0,0,-9.8));
//int blockId = sim->loadURDF("cube.urdf");
//b3BodyInfo bodyInfo;
//sim->getBodyInfo(blockId,&bodyInfo);
sim->loadURDF("plane.urdf");
@@ -34,15 +39,15 @@ int main(int argc, char* argv[])
int minitaurUid = minitaur.setupMinitaur(sim, b3MakeVector3(0,0,.3));
b3RobotSimulatorLoadUrdfFileArgs args;
args.m_startPosition.setValue(2,0,1);
int r2d2 = sim->loadURDF("r2d2.urdf",args);
//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");
}
//b3RobotSimulatorLoadFileResults sdfResults;
//if (!sim->loadSDF("two_cubes.sdf",sdfResults))
//{
// b3Warning("Can't load SDF!\n");
//}
b3Clock clock;
double startTime = clock.getTimeInSeconds();
@@ -69,7 +74,8 @@ int main(int argc, char* argv[])
printf("keyEvent[%d].m_keyCode = %d, state = %d\n", i,keyEvents.m_keyboardEvents[i].m_keyCode,keyEvents.m_keyboardEvents[i].m_keyState);
}
}
b3Clock::usleep(1000*1000);
sim->stepSimulation();
b3Clock::usleep(1000.*1000.*fixedTimeStep);
}
printf("sim->disconnect\n");

522
examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp
View File

@@ -1,17 +1,16 @@
#include "b3RobotSimulatorClientAPI.h"
//#include "SharedMemoryCommands.h"
#include "SharedMemory/PhysicsClientC_API.h"
#ifdef BT_ENABLE_ENET
#include "SharedMemory/PhysicsClientUDP_C_API.h"
#endif//PHYSICS_UDP
#endif //PHYSICS_UDP
#ifdef BT_ENABLE_CLSOCKET
#include "SharedMemory/PhysicsClientTCP_C_API.h"
#endif//PHYSICS_TCP
#endif //PHYSICS_TCP
#include "SharedMemory/PhysicsDirectC_API.h"
@@ -25,7 +24,7 @@ struct b3RobotSimulatorClientAPI_InternalData
b3PhysicsClientHandle m_physicsClientHandle;
b3RobotSimulatorClientAPI_InternalData()
:m_physicsClientHandle(0)
: m_physicsClientHandle(0)
{
}
};
@@ -35,17 +34,16 @@ b3RobotSimulatorClientAPI::b3RobotSimulatorClientAPI()
m_data = new b3RobotSimulatorClientAPI_InternalData();
}
b3RobotSimulatorClientAPI::~b3RobotSimulatorClientAPI()
b3RobotSimulatorClientAPI::~b3RobotSimulatorClientAPI()
{
delete m_data;
delete m_data;
}
bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, int portOrKey)
{
if (m_data->m_physicsClientHandle)
{
b3Warning ("Already connected, disconnect first.");
b3Warning("Already connected, disconnect first.");
return false;
}
b3PhysicsClientHandle sm = 0;
@@ -54,89 +52,88 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
int tcpPort = 6667;
int key = SHARED_MEMORY_KEY;
bool connected = false;
switch (mode)
switch (mode)
{
case eCONNECT_GUI:
case eCONNECT_GUI:
{
int argc = 0;
char* argv[1] = {0};
#ifdef __APPLE__
sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
#else
sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
#endif
break;
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_DIRECT:
{
sm = b3ConnectPhysicsDirect();
break;
}
case eCONNECT_SHARED_MEMORY: {
if (portOrKey>=0)
case eCONNECT_SHARED_MEMORY:
{
if (portOrKey >= 0)
{
key = portOrKey;
}
sm = b3ConnectSharedMemory(key);
break;
sm = b3ConnectSharedMemory(key);
break;
}
case eCONNECT_UDP:
{
if (portOrKey>=0)
case eCONNECT_UDP:
{
if (portOrKey >= 0)
{
udpPort = portOrKey;
}
#ifdef BT_ENABLE_ENET
#ifdef BT_ENABLE_ENET
sm = b3ConnectPhysicsUDP(hostName.c_str(), udpPort);
#else
b3Warning("UDP is not enabled in this build");
#endif //BT_ENABLE_ENET
#else
b3Warning("UDP is not enabled in this build");
#endif //BT_ENABLE_ENET
break;
}
case eCONNECT_TCP:
}
case eCONNECT_TCP:
{
if (portOrKey>=0)
if (portOrKey >= 0)
{
tcpPort = portOrKey;
}
#ifdef BT_ENABLE_CLSOCKET
#ifdef BT_ENABLE_CLSOCKET
sm = b3ConnectPhysicsTCP(hostName.c_str(), tcpPort);
#else
#else
b3Warning("TCP is not enabled in this pybullet build");
#endif //BT_ENABLE_CLSOCKET
#endif //BT_ENABLE_CLSOCKET
break;
}
default: {
b3Warning("connectPhysicsServer unexpected argument");
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;
if (sm)
{
m_data->m_physicsClientHandle = sm;
if (!b3CanSubmitCommand(m_data->m_physicsClientHandle))
{
disconnect();
return false;
}
return true;
}
return false;
}
bool b3RobotSimulatorClientAPI::isConnected() const
{
return (m_data->m_physicsClientHandle!=0);
return (m_data->m_physicsClientHandle != 0);
}
void b3RobotSimulatorClientAPI::disconnect()
{
if (!isConnected())
@@ -157,15 +154,13 @@ void b3RobotSimulatorClientAPI::syncBodies()
return;
}
b3SharedMemoryCommandHandle command;
b3SharedMemoryCommandHandle command;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
command = b3InitSyncBodyInfoCommand(m_data->m_physicsClientHandle);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
statusType = b3GetStatusType(statusHandle);
}
void b3RobotSimulatorClientAPI::resetSimulation()
@@ -175,9 +170,9 @@ void b3RobotSimulatorClientAPI::resetSimulation()
b3Warning("Not connected");
return;
}
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(
m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle));
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(
m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle));
}
bool b3RobotSimulatorClientAPI::canSubmitCommand() const
@@ -186,7 +181,7 @@ bool b3RobotSimulatorClientAPI::canSubmitCommand() const
{
return false;
}
return (b3CanSubmitCommand(m_data->m_physicsClientHandle));
return (b3CanSubmitCommand(m_data->m_physicsClientHandle) != 0);
}
void b3RobotSimulatorClientAPI::stepSimulation()
@@ -200,11 +195,11 @@ void b3RobotSimulatorClientAPI::stepSimulation()
b3SharedMemoryStatusHandle statusHandle;
int statusType;
if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
{
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle));
statusType = b3GetStatusType(statusHandle);
b3Assert(statusType==CMD_STEP_FORWARD_SIMULATION_COMPLETED);
}
{
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle));
statusType = b3GetStatusType(statusHandle);
b3Assert(statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED);
}
}
void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration)
@@ -215,57 +210,26 @@ void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration)
return;
}
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
b3SharedMemoryStatusHandle statusHandle;
b3PhysicsParamSetGravity(command, gravityAcceleration[0],gravityAcceleration[1],gravityAcceleration[2]);
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
b3SharedMemoryStatusHandle statusHandle;
b3PhysicsParamSetGravity(command, gravityAcceleration[0], gravityAcceleration[1], gravityAcceleration[2]);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED);
b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
}
b3Quaternion b3RobotSimulatorClientAPI::getQuaternionFromEuler(const b3Vector3& rollPitchYaw)
{
double phi, the, psi;
phi = rollPitchYaw[0] / 2.0;
the = rollPitchYaw[1] / 2.0;
psi = rollPitchYaw[2] / 2.0;
double quat[4] = {
sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi)};
// normalize the quaternion
double len = sqrt(quat[0] * quat[0] + quat[1] * quat[1] +
quat[2] * quat[2] + quat[3] * quat[3]);
quat[0] /= len;
quat[1] /= len;
quat[2] /= len;
quat[3] /= len;
b3Quaternion q(quat[0],quat[1],quat[2],quat[3]);
b3Quaternion q;
q.setEulerZYX(rollPitchYaw[2],rollPitchYaw[1],rollPitchYaw[0]);
return q;
}
b3Vector3 b3RobotSimulatorClientAPI::getEulerFromQuaternion(const b3Quaternion& quat)
{
double squ;
double sqx;
double sqy;
double sqz;
double sarg;
b3Vector3 rpy;
sqx = quat[0] * quat[0];
sqy = quat[1] * quat[1];
sqz = quat[2] * quat[2];
squ = quat[3] * quat[3];
rpy[0] = atan2(2 * (quat[1] * quat[2] + quat[3] * quat[0]),
squ - sqx - sqy + sqz);
sarg = -2 * (quat[0] * quat[2] - quat[3] * quat[1]);
rpy[1] = sarg <= -1.0 ? -0.5 * 3.141592538
: (sarg >= 1.0 ? 0.5 * 3.141592538 : asin(sarg));
rpy[2] = atan2(2 * (quat[0] * quat[1] + quat[3] * quat[2]),
squ + sqx - sqy - sqz);
return rpy;
b3Scalar roll,pitch,yaw;
quat.getEulerZYX(yaw,pitch,roll);
b3Vector3 rpy2 = b3MakeVector3(roll,pitch,yaw);
return rpy2;
}
int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args)
@@ -278,31 +242,26 @@ int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struc
return robotUniqueId;
}
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
//setting the initial position, orientation and other arguments are optional
b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0],
args.m_startPosition[1],
args.m_startPosition[2]);
b3LoadUrdfCommandSetStartOrientation(command,args.m_startOrientation[0]
,args.m_startOrientation[1]
,args.m_startOrientation[2]
,args.m_startOrientation[3]);
args.m_startPosition[1],
args.m_startPosition[2]);
b3LoadUrdfCommandSetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]);
if (args.m_forceOverrideFixedBase)
{
b3LoadUrdfCommandSetUseFixedBase(command,true);
b3LoadUrdfCommandSetUseFixedBase(command, true);
}
b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
statusType = b3GetStatusType(statusHandle);
b3Assert(statusType==CMD_URDF_LOADING_COMPLETED);
if (statusType==CMD_URDF_LOADING_COMPLETED)
b3Assert(statusType == CMD_URDF_LOADING_COMPLETED);
if (statusType == CMD_URDF_LOADING_COMPLETED)
{
robotUniqueId = b3GetStatusBodyIndex(statusHandle);
}
@@ -329,12 +288,12 @@ bool b3RobotSimulatorClientAPI::loadMJCF(const std::string& fileName, b3RobotSim
b3Warning("Couldn't load .mjcf file.");
return false;
}
int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0);
int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
if (numBodies)
{
results.m_uniqueObjectIds.resize(numBodies);
int numBodies;
numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size());
numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
}
return true;
@@ -359,12 +318,12 @@ bool b3RobotSimulatorClientAPI::loadBullet(const std::string& fileName, b3RobotS
{
return false;
}
int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0);
int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
if (numBodies)
{
results.m_uniqueObjectIds.resize(numBodies);
int numBodies;
numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size());
numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
}
return true;
@@ -383,19 +342,18 @@ bool b3RobotSimulatorClientAPI::loadSDF(const std::string& fileName, b3RobotSimu
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody);
b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
statusType = b3GetStatusType(statusHandle);
b3Assert(statusType == CMD_SDF_LOADING_COMPLETED);
if (statusType == CMD_SDF_LOADING_COMPLETED)
{
int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0);
int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
if (numBodies)
{
results.m_uniqueObjectIds.resize(numBodies);
int numBodies;
numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size());
numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
}
statusOk = true;
}
@@ -411,7 +369,7 @@ bool b3RobotSimulatorClientAPI::getBodyInfo(int bodyUniqueId, struct b3BodyInfo*
return false;
}
int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo);
int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo);
return (result != 0);
}
@@ -431,7 +389,8 @@ bool b3RobotSimulatorClientAPI::getBasePositionAndOrientation(int bodyUniqueId,
const int status_type = b3GetStatusType(status_handle);
const double* actualStateQ;
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
return false;
}
@@ -466,14 +425,72 @@ bool b3RobotSimulatorClientAPI::resetBasePositionAndOrientation(int bodyUniqueId
b3CreatePoseCommandSetBasePosition(commandHandle, basePosition[0], basePosition[1], basePosition[2]);
b3CreatePoseCommandSetBaseOrientation(commandHandle, baseOrientation[0], baseOrientation[1],
baseOrientation[2], baseOrientation[3]);
baseOrientation[2], baseOrientation[3]);
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
return true;
}
bool b3RobotSimulatorClientAPI::getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const
{
if (!isConnected())
{
b3Warning("Not connected");
return false;
}
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
const int status_type = b3GetStatusType(statusHandle);
const double* actualStateQdot;
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
return false;
}
b3GetStatusActualState(statusHandle, 0 /* body_unique_id */,
0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */,
0 /*root_local_inertial_frame*/, 0,
&actualStateQdot, 0 /* joint_reaction_forces */);
baseLinearVelocity[0] = actualStateQdot[0];
baseLinearVelocity[1] = actualStateQdot[1];
baseLinearVelocity[2] = actualStateQdot[2];
baseAngularVelocity[0] = actualStateQdot[3];
baseAngularVelocity[1] = actualStateQdot[4];
baseAngularVelocity[2] = actualStateQdot[5];
return true;
}
bool b3RobotSimulatorClientAPI::resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const
{
if (!isConnected())
{
b3Warning("Not connected");
return false;
}
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
b3Vector3DoubleData linVelDouble;
linearVelocity.serializeDouble(linVelDouble);
b3CreatePoseCommandSetBaseLinearVelocity(commandHandle, linVelDouble.m_floats);
b3Vector3DoubleData angVelDouble;
angularVelocity.serializeDouble(angVelDouble);
b3CreatePoseCommandSetBaseAngularVelocity(commandHandle, angVelDouble.m_floats);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
return true;
}
void b3RobotSimulatorClientAPI::setRealTimeSimulation(bool enableRealTimeSimulation)
{
@@ -500,10 +517,9 @@ int b3RobotSimulatorClientAPI::getNumJoints(int bodyUniqueId) const
b3Warning("Not connected");
return false;
}
return b3GetNumJoints(m_data->m_physicsClientHandle,bodyUniqueId);
return b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
}
bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo)
{
if (!isConnected())
@@ -511,7 +527,7 @@ bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b
b3Warning("Not connected");
return false;
}
return (b3GetJointInfo(m_data->m_physicsClientHandle,bodyUniqueId, jointIndex,jointInfo)!=0);
return (b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, jointInfo) != 0);
}
void b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo)
@@ -521,55 +537,55 @@ void b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parent
b3Warning("Not connected");
return;
}
b3SharedMemoryStatusHandle statusHandle;
b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle));
if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
{
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitCreateUserConstraintCommand(m_data->m_physicsClientHandle, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo));
}
b3SharedMemoryStatusHandle statusHandle;
b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle));
if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
{
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitCreateUserConstraintCommand(m_data->m_physicsClientHandle, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo));
}
}
bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState *state)
bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state)
{
if (!isConnected())
{
b3Warning("Not connected");
return false;
}
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
int statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
int statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
if (b3GetJointState(m_data->m_physicsClientHandle, statusHandle, jointIndex, state))
{
return true;
}
if (b3GetJointState(m_data->m_physicsClientHandle, statusHandle, jointIndex, state))
{
return true;
}
}
return false;
return false;
}
bool b3RobotSimulatorClientAPI::resetJointState(int bodyUniqueId, int jointIndex, double targetValue)
{
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
int numJoints;
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
int numJoints;
numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
if ((jointIndex >= numJoints) || (jointIndex < 0)) {
return false;
}
numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
if ((jointIndex >= numJoints) || (jointIndex < 0))
{
return false;
}
commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex,
targetValue);
b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex,
targetValue);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
return false;
}
void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3RobotSimulatorJointMotorArgs& args)
{
if (!isConnected())
@@ -582,39 +598,39 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint
{
case CONTROL_MODE_VELOCITY:
{
b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_VELOCITY);
b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_VELOCITY);
b3JointInfo jointInfo;
b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
int uIndex = jointInfo.m_uIndex;
b3JointControlSetKd(command,uIndex,args.m_kd);
b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity);
b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue);
b3JointControlSetKd(command, uIndex, args.m_kd);
b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
break;
}
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD);
b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD);
b3JointInfo jointInfo;
b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
int uIndex = jointInfo.m_uIndex;
int qIndex = jointInfo.m_qIndex;
b3JointControlSetDesiredPosition(command,qIndex,args.m_targetPosition);
b3JointControlSetKp(command,uIndex,args.m_kp);
b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity);
b3JointControlSetKd(command,uIndex,args.m_kd);
b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue);
b3JointControlSetDesiredPosition(command, qIndex, args.m_targetPosition);
b3JointControlSetKp(command, uIndex, args.m_kp);
b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
b3JointControlSetKd(command, uIndex, args.m_kd);
b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
break;
}
case CONTROL_MODE_TORQUE:
{
b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_TORQUE);
b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_TORQUE);
b3JointInfo jointInfo;
b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
int uIndex = jointInfo.m_uIndex;
b3JointControlSetDesiredForceTorque(command,uIndex,args.m_maxTorqueValue);
b3JointControlSetDesiredForceTorque(command, uIndex, args.m_maxTorqueValue);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
break;
}
@@ -625,7 +641,6 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint
}
}
void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations)
{
if (!isConnected())
@@ -633,12 +648,11 @@ void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations)
b3Warning("Not connected");
return;
}
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
b3SharedMemoryStatusHandle statusHandle;
b3PhysicsParamSetNumSolverIterations(command, numIterations);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED);
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
b3SharedMemoryStatusHandle statusHandle;
b3PhysicsParamSetNumSolverIterations(command, numIterations);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
}
void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds)
@@ -654,10 +668,8 @@ void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds)
int ret;
ret = b3PhysicsParamSetTimeStep(command, timeStepInSeconds);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
}
void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps)
{
if (!isConnected())
@@ -665,14 +677,13 @@ void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps)
b3Warning("Not connected");
return;
}
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
b3SharedMemoryStatusHandle statusHandle;
b3PhysicsParamSetNumSubSteps(command, numSubSteps);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED);
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
b3SharedMemoryStatusHandle statusHandle;
b3PhysicsParamSetNumSubSteps(command, numSubSteps);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
}
bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results)
{
if (!isConnected())
@@ -680,55 +691,52 @@ bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotS
b3Warning("Not connected");
return false;
}
b3Assert(args.m_endEffectorLinkIndex>=0);
b3Assert(args.m_bodyUniqueId>=0);
b3Assert(args.m_endEffectorLinkIndex >= 0);
b3Assert(args.m_bodyUniqueId >= 0);
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClientHandle,args.m_bodyUniqueId);
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClientHandle, args.m_bodyUniqueId);
if ((args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) && (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY))
{
b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
} else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION)
{
b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation);
} else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)
{
b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
} else
{
b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition);
b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
}
else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION)
{
b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation);
}
else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)
{
b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
}
else
{
b3CalculateInverseKinematicsAddTargetPurePosition(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition);
}
if (args.m_flags & B3_HAS_JOINT_DAMPING)
{
b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]);
}
b3SharedMemoryStatusHandle statusHandle;
if (args.m_flags & B3_HAS_JOINT_DAMPING)
{
b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]);
}
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
int numPos = 0;
int numPos=0;
bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
&results.m_bodyUniqueId,
&numPos,
0)!=0;
if (result && numPos)
{
results.m_calculatedJointPositions.resize(numPos);
result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
&results.m_bodyUniqueId,
&numPos,
&results.m_calculatedJointPositions[0])!=0;
}
&results.m_bodyUniqueId,
&numPos,
0) != 0;
if (result && numPos)
{
results.m_calculatedJointPositions.resize(numPos);
result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
&results.m_bodyUniqueId,
&numPos,
&results.m_calculatedJointPositions[0]) != 0;
}
return result;
}
bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian)
{
if (!isConnected())
@@ -736,12 +744,12 @@ bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex,
b3Warning("Not connected");
return false;
}
b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED)
{
b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian);
b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED)
{
b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian);
return true;
}
return false;
@@ -754,14 +762,14 @@ bool b3RobotSimulatorClientAPI::getLinkState(int bodyUniqueId, int linkIndex, b3
b3Warning("Not connected");
return false;
}
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState);
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState);
return true;
}
}
return false;
}
@@ -787,9 +795,9 @@ void b3RobotSimulatorClientAPI::getVREvents(b3VREventsData* vrEventsData)
return;
}
b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle);
b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle);
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData);
b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData);
}
void b3RobotSimulatorClientAPI::getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData)
@@ -802,32 +810,35 @@ void b3RobotSimulatorClientAPI::getKeyboardEvents(b3KeyboardEventsData* keyboard
return;
}
b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle);
b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle);
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
b3GetKeyboardEventsData(m_data->m_physicsClientHandle,keyboardEventsData);
b3GetKeyboardEventsData(m_data->m_physicsClientHandle, keyboardEventsData);
}
int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds)
int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds, int maxLogDof)
{
int loggingUniqueId = -1;
b3SharedMemoryCommandHandle commandHandle;
commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle);
b3StateLoggingStart(commandHandle,loggingType,fileName.c_str());
for ( int i=0;i<objectUniqueIds.size();i++)
b3StateLoggingStart(commandHandle, loggingType, fileName.c_str());
for (int i = 0; i < objectUniqueIds.size(); i++)
{
int objectUid = objectUniqueIds[i];
b3StateLoggingAddLoggingObjectUniqueId(commandHandle,objectUid);
b3StateLoggingAddLoggingObjectUniqueId(commandHandle, objectUid);
}
if (maxLogDof > 0)
{
b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof);
}
b3SharedMemoryStatusHandle statusHandle;
int statusType;
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType==CMD_STATE_LOGGING_START_COMPLETED)
if (statusType == CMD_STATE_LOGGING_START_COMPLETED)
{
loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle);
}
@@ -842,4 +853,3 @@ void b3RobotSimulatorClientAPI::stopStateLogging(int stateLoggerUniqueId)
b3StateLoggingStop(commandHandle, stateLoggerUniqueId);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
}

118
examples/RobotSimulator/b3RobotSimulatorClientAPI.h
View File

@@ -9,23 +9,18 @@
#include <string>
struct b3RobotSimulatorLoadUrdfFileArgs
{
b3Vector3 m_startPosition;
b3Quaternion m_startOrientation;
bool m_forceOverrideFixedBase;
bool m_useMultiBody;
bool m_useMultiBody;
b3RobotSimulatorLoadUrdfFileArgs()
:
m_startPosition(b3MakeVector3(0,0,0)),
m_startOrientation(b3Quaternion(0,0,0,1)),
m_forceOverrideFixedBase(false),
m_useMultiBody(true)
: m_startPosition(b3MakeVector3(0, 0, 0)),
m_startOrientation(b3Quaternion(0, 0, 0, 1)),
m_forceOverrideFixedBase(false),
m_useMultiBody(true)
{
}
};
@@ -33,17 +28,15 @@ struct b3RobotSimulatorLoadUrdfFileArgs
struct b3RobotSimulatorLoadSdfFileArgs
{
bool m_forceOverrideFixedBase;
bool m_useMultiBody;
bool m_useMultiBody;
b3RobotSimulatorLoadSdfFileArgs()
:
m_forceOverrideFixedBase(false),
m_useMultiBody(true)
: m_forceOverrideFixedBase(false),
m_useMultiBody(true)
{
}
};
struct b3RobotSimulatorLoadFileResults
{
b3AlignedObjectArray<int> m_uniqueObjectIds;
@@ -55,7 +48,7 @@ struct b3RobotSimulatorLoadFileResults
struct b3RobotSimulatorJointMotorArgs
{
int m_controlMode;
double m_targetPosition;
double m_kp;
@@ -65,52 +58,52 @@ struct b3RobotSimulatorJointMotorArgs
double m_maxTorqueValue;
b3RobotSimulatorJointMotorArgs(int controlMode)
:m_controlMode(controlMode),
m_targetPosition(0),
m_kp(0.1),
m_targetVelocity(0),
m_kd(0.9),
m_maxTorqueValue(1000)
: m_controlMode(controlMode),
m_targetPosition(0),
m_kp(0.1),
m_targetVelocity(0),
m_kd(0.9),
m_maxTorqueValue(1000)
{
}
};
enum b3RobotSimulatorInverseKinematicsFlags
{
B3_HAS_IK_TARGET_ORIENTATION=1,
B3_HAS_NULL_SPACE_VELOCITY=2,
B3_HAS_JOINT_DAMPING=4,
B3_HAS_IK_TARGET_ORIENTATION = 1,
B3_HAS_NULL_SPACE_VELOCITY = 2,
B3_HAS_JOINT_DAMPING = 4,
};
struct b3RobotSimulatorInverseKinematicArgs
{
int m_bodyUniqueId;
// double* m_currentJointPositions;
// int m_numPositions;
// double* m_currentJointPositions;
// int m_numPositions;
double m_endEffectorTargetPosition[3];
double m_endEffectorTargetOrientation[4];
int m_endEffectorLinkIndex;
int m_endEffectorLinkIndex;
int m_flags;
int m_numDegreeOfFreedom;
b3AlignedObjectArray<double> m_lowerLimits;
b3AlignedObjectArray<double> m_upperLimits;
b3AlignedObjectArray<double> m_jointRanges;
b3AlignedObjectArray<double> m_restPoses;
b3AlignedObjectArray<double> m_jointDamping;
int m_numDegreeOfFreedom;
b3AlignedObjectArray<double> m_lowerLimits;
b3AlignedObjectArray<double> m_upperLimits;
b3AlignedObjectArray<double> m_jointRanges;
b3AlignedObjectArray<double> m_restPoses;
b3AlignedObjectArray<double> m_jointDamping;
b3RobotSimulatorInverseKinematicArgs()
:m_bodyUniqueId(-1),
m_endEffectorLinkIndex(-1),
m_flags(0)
: m_bodyUniqueId(-1),
m_endEffectorLinkIndex(-1),
m_flags(0)
{
m_endEffectorTargetPosition[0]=0;
m_endEffectorTargetPosition[1]=0;
m_endEffectorTargetPosition[2]=0;
m_endEffectorTargetPosition[0] = 0;
m_endEffectorTargetPosition[1] = 0;
m_endEffectorTargetPosition[2] = 0;
m_endEffectorTargetOrientation[0]=0;
m_endEffectorTargetOrientation[1]=0;
m_endEffectorTargetOrientation[2]=0;
m_endEffectorTargetOrientation[3]=1;
m_endEffectorTargetOrientation[0] = 0;
m_endEffectorTargetOrientation[1] = 0;
m_endEffectorTargetOrientation[2] = 0;
m_endEffectorTargetOrientation[3] = 1;
}
};
@@ -120,20 +113,19 @@ struct b3RobotSimulatorInverseKinematicsResults
b3AlignedObjectArray<double> m_calculatedJointPositions;
};
///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet
///as documented in the pybullet Quickstart Guide
///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
class b3RobotSimulatorClientAPI
{
struct b3RobotSimulatorClientAPI_InternalData* m_data;
public:
b3RobotSimulatorClientAPI();
virtual ~b3RobotSimulatorClientAPI();
bool connect(int mode, const std::string& hostName="localhost", int portOrKey=-1);
bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1);
void disconnect();
bool isConnected() const;
@@ -145,8 +137,8 @@ public:
b3Quaternion getQuaternionFromEuler(const b3Vector3& rollPitchYaw);
b3Vector3 getEulerFromQuaternion(const b3Quaternion& quat);
int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args=b3RobotSimulatorLoadUrdfFileArgs());
bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results=b3RobotSimulatorLoadFileResults(), const struct b3RobotSimulatorLoadSdfFileArgs& args=b3RobotSimulatorLoadSdfFileArgs());
int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args = b3RobotSimulatorLoadUrdfFileArgs());
bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results = b3RobotSimulatorLoadFileResults(), const struct b3RobotSimulatorLoadSdfFileArgs& args = b3RobotSimulatorLoadSdfFileArgs());
bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
@@ -155,14 +147,17 @@ public:
bool getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const;
bool resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation);
bool getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const;
bool resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const;
int getNumJoints(int bodyUniqueId) const;
bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo);
void createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo);
bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState *state);
void createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo);
bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state);
bool resetJointState(int bodyUniqueId, int jointIndex, double targetValue);
void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs& args);
@@ -170,29 +165,28 @@ public:
void stepSimulation();
bool canSubmitCommand() const;
void setRealTimeSimulation(bool enableRealTimeSimulation);
void setGravity(const b3Vector3& gravityAcceleration);
void setTimeStep(double timeStepInSeconds);
void setNumSimulationSubSteps(int numSubSteps);
void setNumSimulationSubSteps(int numSubSteps);
void setNumSolverIterations(int numIterations);
bool calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results);
bool getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian);
bool getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian);
bool getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState);
void configureDebugVisualizer(enum b3ConfigureDebugVisualizerEnum flag, int enable);
int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds);
int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds, int maxLogDof = -1);
void stopStateLogging(int stateLoggerUniqueId);
void getVREvents(b3VREventsData* vrEventsData);
void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData);
};
#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H
#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H