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

94
examples/RobotSimulator/MinitaurSetup.cpp
View File

@@ -49,57 +49,69 @@ void MinitaurSetup::resetPose(class b3RobotSimulatorClientAPI* sim)
sim->setJointMotorControl(m_data->m_quadrupedUniqueId,i,controlArgs); sim->setJointMotorControl(m_data->m_quadrupedUniqueId,i,controlArgs);
} }
//right front leg b3Scalar startAngle = B3_HALF_PI;
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightR_joint"],1.57); b3Scalar upperLegLength = 11.5;
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightR_link"],-2.2); b3Scalar lowerLegLength = 20;
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightL_joint"],-1.57); b3Scalar kneeAngle = B3_PI+b3Acos(upperLegLength/lowerLegLength);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],2.2);
b3Scalar motorDirs[8] = {-1,-1,-1,-1,1,1,1,1};
b3JointInfo jointInfo; b3JointInfo jointInfo;
jointInfo.m_jointType = ePoint2PointType; 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 //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["motor_front_leftL_joint"],motorDirs[0] * startAngle);
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["knee_front_leftL_link"],motorDirs[0]*kneeAngle);
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["motor_front_leftR_joint"],motorDirs[1] * startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftL_link"],2.2); 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.01; jointInfo.m_parentFrame[2] = 0.2;
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.015; jointInfo.m_childFrame[2] = 0.2; 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"], 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); 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", motorDirs[0] * startAngle);
setDesiredMotorAngle(sim,"motor_front_leftL_joint", -1.57); setDesiredMotorAngle(sim,"motor_front_leftR_joint", motorDirs[1] * startAngle);
//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);
//left back leg //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["motor_back_leftL_joint"],motorDirs[2] * startAngle);
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["knee_back_leftL_link"],motorDirs[2] * kneeAngle);
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["motor_back_leftR_joint"],motorDirs[3] * startAngle);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftL_link"],2.2); 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.01; jointInfo.m_parentFrame[2] = 0.2; 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.015; jointInfo.m_childFrame[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"], 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); 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", motorDirs[2] * startAngle);
setDesiredMotorAngle(sim,"motor_back_leftL_joint", -1.57); 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) 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_startPosition = startPos;
args.m_startOrientation = startOrn; 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); int numJoints = sim->getNumJoints(m_data->m_quadrupedUniqueId);
for (int i=0;i<numJoints;i++) for (int i=0;i<numJoints;i++)

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

290
examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp
View File

@@ -1,17 +1,16 @@
#include "b3RobotSimulatorClientAPI.h" #include "b3RobotSimulatorClientAPI.h"
//#include "SharedMemoryCommands.h" //#include "SharedMemoryCommands.h"
#include "SharedMemory/PhysicsClientC_API.h" #include "SharedMemory/PhysicsClientC_API.h"
#ifdef BT_ENABLE_ENET #ifdef BT_ENABLE_ENET
#include "SharedMemory/PhysicsClientUDP_C_API.h" #include "SharedMemory/PhysicsClientUDP_C_API.h"
#endif//PHYSICS_UDP #endif //PHYSICS_UDP
#ifdef BT_ENABLE_CLSOCKET #ifdef BT_ENABLE_CLSOCKET
#include "SharedMemory/PhysicsClientTCP_C_API.h" #include "SharedMemory/PhysicsClientTCP_C_API.h"
#endif//PHYSICS_TCP #endif //PHYSICS_TCP
#include "SharedMemory/PhysicsDirectC_API.h" #include "SharedMemory/PhysicsDirectC_API.h"
@@ -25,7 +24,7 @@ struct b3RobotSimulatorClientAPI_InternalData
b3PhysicsClientHandle m_physicsClientHandle; b3PhysicsClientHandle m_physicsClientHandle;
b3RobotSimulatorClientAPI_InternalData() b3RobotSimulatorClientAPI_InternalData()
:m_physicsClientHandle(0) : m_physicsClientHandle(0)
{ {
} }
}; };
@@ -35,17 +34,16 @@ b3RobotSimulatorClientAPI::b3RobotSimulatorClientAPI()
m_data = new b3RobotSimulatorClientAPI_InternalData(); 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) bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, int portOrKey)
{ {
if (m_data->m_physicsClientHandle) if (m_data->m_physicsClientHandle)
{ {
b3Warning ("Already connected, disconnect first."); b3Warning("Already connected, disconnect first.");
return false; return false;
} }
b3PhysicsClientHandle sm = 0; b3PhysicsClientHandle sm = 0;
@@ -55,26 +53,27 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
int key = SHARED_MEMORY_KEY; int key = SHARED_MEMORY_KEY;
bool connected = false; bool connected = false;
switch (mode) switch (mode)
{ {
case eCONNECT_GUI: case eCONNECT_GUI:
{ {
int argc = 0; int argc = 0;
char* argv[1] = {0}; char* argv[1] = {0};
#ifdef __APPLE__ #ifdef __APPLE__
sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv); sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
#else #else
sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv); sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
#endif #endif
break; break;
} }
case eCONNECT_DIRECT: { case eCONNECT_DIRECT:
{
sm = b3ConnectPhysicsDirect(); sm = b3ConnectPhysicsDirect();
break; break;
} }
case eCONNECT_SHARED_MEMORY: { case eCONNECT_SHARED_MEMORY:
if (portOrKey>=0) {
if (portOrKey >= 0)
{ {
key = portOrKey; key = portOrKey;
} }
@@ -83,37 +82,36 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
} }
case eCONNECT_UDP: case eCONNECT_UDP:
{ {
if (portOrKey>=0) if (portOrKey >= 0)
{ {
udpPort = portOrKey; udpPort = portOrKey;
} }
#ifdef BT_ENABLE_ENET #ifdef BT_ENABLE_ENET
sm = b3ConnectPhysicsUDP(hostName.c_str(), udpPort); sm = b3ConnectPhysicsUDP(hostName.c_str(), udpPort);
#else #else
b3Warning("UDP is not enabled in this build"); b3Warning("UDP is not enabled in this build");
#endif //BT_ENABLE_ENET #endif //BT_ENABLE_ENET
break; break;
} }
case eCONNECT_TCP: case eCONNECT_TCP:
{ {
if (portOrKey>=0) if (portOrKey >= 0)
{ {
tcpPort = portOrKey; tcpPort = portOrKey;
} }
#ifdef BT_ENABLE_CLSOCKET #ifdef BT_ENABLE_CLSOCKET
sm = b3ConnectPhysicsTCP(hostName.c_str(), tcpPort); sm = b3ConnectPhysicsTCP(hostName.c_str(), tcpPort);
#else #else
b3Warning("TCP is not enabled in this pybullet build"); b3Warning("TCP is not enabled in this pybullet build");
#endif //BT_ENABLE_CLSOCKET #endif //BT_ENABLE_CLSOCKET
break; break;
} }
default:
{
default: {
b3Warning("connectPhysicsServer unexpected argument"); b3Warning("connectPhysicsServer unexpected argument");
} }
}; };
@@ -133,10 +131,9 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
bool b3RobotSimulatorClientAPI::isConnected() const bool b3RobotSimulatorClientAPI::isConnected() const
{ {
return (m_data->m_physicsClientHandle!=0); return (m_data->m_physicsClientHandle != 0);
} }
void b3RobotSimulatorClientAPI::disconnect() void b3RobotSimulatorClientAPI::disconnect()
{ {
if (!isConnected()) if (!isConnected())
@@ -157,7 +154,6 @@ void b3RobotSimulatorClientAPI::syncBodies()
return; return;
} }
b3SharedMemoryCommandHandle command; b3SharedMemoryCommandHandle command;
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
int statusType; int statusType;
@@ -165,7 +161,6 @@ void b3RobotSimulatorClientAPI::syncBodies()
command = b3InitSyncBodyInfoCommand(m_data->m_physicsClientHandle); command = b3InitSyncBodyInfoCommand(m_data->m_physicsClientHandle);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
statusType = b3GetStatusType(statusHandle); statusType = b3GetStatusType(statusHandle);
} }
void b3RobotSimulatorClientAPI::resetSimulation() void b3RobotSimulatorClientAPI::resetSimulation()
@@ -186,7 +181,7 @@ bool b3RobotSimulatorClientAPI::canSubmitCommand() const
{ {
return false; return false;
} }
return (b3CanSubmitCommand(m_data->m_physicsClientHandle)); return (b3CanSubmitCommand(m_data->m_physicsClientHandle) != 0);
} }
void b3RobotSimulatorClientAPI::stepSimulation() void b3RobotSimulatorClientAPI::stepSimulation()
@@ -203,7 +198,7 @@ void b3RobotSimulatorClientAPI::stepSimulation()
{ {
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle)); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle));
statusType = b3GetStatusType(statusHandle); statusType = b3GetStatusType(statusHandle);
b3Assert(statusType==CMD_STEP_FORWARD_SIMULATION_COMPLETED); b3Assert(statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED);
} }
} }
@@ -217,55 +212,24 @@ void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration)
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
b3PhysicsParamSetGravity(command, gravityAcceleration[0],gravityAcceleration[1],gravityAcceleration[2]); b3PhysicsParamSetGravity(command, gravityAcceleration[0], gravityAcceleration[1], gravityAcceleration[2]);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); 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) b3Quaternion b3RobotSimulatorClientAPI::getQuaternionFromEuler(const b3Vector3& rollPitchYaw)
{ {
double phi, the, psi; b3Quaternion q;
phi = rollPitchYaw[0] / 2.0; q.setEulerZYX(rollPitchYaw[2],rollPitchYaw[1],rollPitchYaw[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]);
return q; return q;
} }
b3Vector3 b3RobotSimulatorClientAPI::getEulerFromQuaternion(const b3Quaternion& quat) b3Vector3 b3RobotSimulatorClientAPI::getEulerFromQuaternion(const b3Quaternion& quat)
{ {
double squ; b3Scalar roll,pitch,yaw;
double sqx; quat.getEulerZYX(yaw,pitch,roll);
double sqy; b3Vector3 rpy2 = b3MakeVector3(roll,pitch,yaw);
double sqz; return rpy2;
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;
} }
int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args) int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args)
@@ -278,8 +242,6 @@ int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struc
return robotUniqueId; return robotUniqueId;
} }
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
int statusType; int statusType;
b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
@@ -289,20 +251,17 @@ int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struc
b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0], b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0],
args.m_startPosition[1], args.m_startPosition[1],
args.m_startPosition[2]); args.m_startPosition[2]);
b3LoadUrdfCommandSetStartOrientation(command,args.m_startOrientation[0] b3LoadUrdfCommandSetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]);
,args.m_startOrientation[1]
,args.m_startOrientation[2]
,args.m_startOrientation[3]);
if (args.m_forceOverrideFixedBase) if (args.m_forceOverrideFixedBase)
{ {
b3LoadUrdfCommandSetUseFixedBase(command,true); b3LoadUrdfCommandSetUseFixedBase(command, true);
} }
b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody); b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
statusType = b3GetStatusType(statusHandle); statusType = b3GetStatusType(statusHandle);
b3Assert(statusType==CMD_URDF_LOADING_COMPLETED); b3Assert(statusType == CMD_URDF_LOADING_COMPLETED);
if (statusType==CMD_URDF_LOADING_COMPLETED) if (statusType == CMD_URDF_LOADING_COMPLETED)
{ {
robotUniqueId = b3GetStatusBodyIndex(statusHandle); robotUniqueId = b3GetStatusBodyIndex(statusHandle);
} }
@@ -329,12 +288,12 @@ bool b3RobotSimulatorClientAPI::loadMJCF(const std::string& fileName, b3RobotSim
b3Warning("Couldn't load .mjcf file."); b3Warning("Couldn't load .mjcf file.");
return false; return false;
} }
int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0); int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
if (numBodies) if (numBodies)
{ {
results.m_uniqueObjectIds.resize(numBodies); results.m_uniqueObjectIds.resize(numBodies);
int 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; return true;
@@ -359,12 +318,12 @@ bool b3RobotSimulatorClientAPI::loadBullet(const std::string& fileName, b3RobotS
{ {
return false; return false;
} }
int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0); int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
if (numBodies) if (numBodies)
{ {
results.m_uniqueObjectIds.resize(numBodies); results.m_uniqueObjectIds.resize(numBodies);
int 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; return true;
@@ -389,13 +348,12 @@ bool b3RobotSimulatorClientAPI::loadSDF(const std::string& fileName, b3RobotSimu
b3Assert(statusType == CMD_SDF_LOADING_COMPLETED); b3Assert(statusType == CMD_SDF_LOADING_COMPLETED);
if (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) if (numBodies)
{ {
results.m_uniqueObjectIds.resize(numBodies); results.m_uniqueObjectIds.resize(numBodies);
int 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; statusOk = true;
} }
@@ -431,7 +389,8 @@ bool b3RobotSimulatorClientAPI::getBasePositionAndOrientation(int bodyUniqueId,
const int status_type = b3GetStatusType(status_handle); const int status_type = b3GetStatusType(status_handle);
const double* actualStateQ; const double* actualStateQ;
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
return false; return false;
} }
@@ -474,6 +433,64 @@ bool b3RobotSimulatorClientAPI::resetBasePositionAndOrientation(int bodyUniqueId
return true; 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) void b3RobotSimulatorClientAPI::setRealTimeSimulation(bool enableRealTimeSimulation)
{ {
@@ -500,10 +517,9 @@ int b3RobotSimulatorClientAPI::getNumJoints(int bodyUniqueId) const
b3Warning("Not connected"); b3Warning("Not connected");
return false; 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) bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo)
{ {
if (!isConnected()) if (!isConnected())
@@ -511,7 +527,7 @@ bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b
b3Warning("Not connected"); b3Warning("Not connected");
return false; 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) void b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo)
@@ -529,14 +545,14 @@ void b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parent
} }
} }
bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState *state) bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state)
{ {
if (!isConnected()) if (!isConnected())
{ {
b3Warning("Not connected"); b3Warning("Not connected");
return false; return false;
} }
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId); b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
int statusType = b3GetStatusType(statusHandle); int statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED) if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
@@ -556,7 +572,8 @@ bool b3RobotSimulatorClientAPI::resetJointState(int bodyUniqueId, int jointIndex
int numJoints; int numJoints;
numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId); numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
if ((jointIndex >= numJoints) || (jointIndex < 0)) { if ((jointIndex >= numJoints) || (jointIndex < 0))
{
return false; return false;
} }
@@ -569,7 +586,6 @@ bool b3RobotSimulatorClientAPI::resetJointState(int bodyUniqueId, int jointIndex
return false; return false;
} }
void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3RobotSimulatorJointMotorArgs& args) void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3RobotSimulatorJointMotorArgs& args)
{ {
if (!isConnected()) if (!isConnected())
@@ -582,39 +598,39 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint
{ {
case CONTROL_MODE_VELOCITY: 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; b3JointInfo jointInfo;
b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo); b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
int uIndex = jointInfo.m_uIndex; int uIndex = jointInfo.m_uIndex;
b3JointControlSetKd(command,uIndex,args.m_kd); b3JointControlSetKd(command, uIndex, args.m_kd);
b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity); b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue); b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
break; break;
} }
case CONTROL_MODE_POSITION_VELOCITY_PD: 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; b3JointInfo jointInfo;
b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo); b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
int uIndex = jointInfo.m_uIndex; int uIndex = jointInfo.m_uIndex;
int qIndex = jointInfo.m_qIndex; int qIndex = jointInfo.m_qIndex;
b3JointControlSetDesiredPosition(command,qIndex,args.m_targetPosition); b3JointControlSetDesiredPosition(command, qIndex, args.m_targetPosition);
b3JointControlSetKp(command,uIndex,args.m_kp); b3JointControlSetKp(command, uIndex, args.m_kp);
b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity); b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
b3JointControlSetKd(command,uIndex,args.m_kd); b3JointControlSetKd(command, uIndex, args.m_kd);
b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue); b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
break; break;
} }
case CONTROL_MODE_TORQUE: 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; b3JointInfo jointInfo;
b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo); b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
int uIndex = jointInfo.m_uIndex; int uIndex = jointInfo.m_uIndex;
b3JointControlSetDesiredForceTorque(command,uIndex,args.m_maxTorqueValue); b3JointControlSetDesiredForceTorque(command, uIndex, args.m_maxTorqueValue);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
break; break;
} }
@@ -625,7 +641,6 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint
} }
} }
void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations) void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations)
{ {
if (!isConnected()) if (!isConnected())
@@ -637,8 +652,7 @@ void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations)
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
b3PhysicsParamSetNumSolverIterations(command, numIterations); b3PhysicsParamSetNumSolverIterations(command, numIterations);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
} }
void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds) void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds)
@@ -654,10 +668,8 @@ void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds)
int ret; int ret;
ret = b3PhysicsParamSetTimeStep(command, timeStepInSeconds); ret = b3PhysicsParamSetTimeStep(command, timeStepInSeconds);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
} }
void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps) void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps)
{ {
if (!isConnected()) if (!isConnected())
@@ -669,10 +681,9 @@ void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps)
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
b3PhysicsParamSetNumSubSteps(command, numSubSteps); b3PhysicsParamSetNumSubSteps(command, numSubSteps);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
} }
bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results) bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results)
{ {
if (!isConnected()) if (!isConnected())
@@ -680,23 +691,25 @@ bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotS
b3Warning("Not connected"); b3Warning("Not connected");
return false; return false;
} }
b3Assert(args.m_endEffectorLinkIndex>=0); b3Assert(args.m_endEffectorLinkIndex >= 0);
b3Assert(args.m_bodyUniqueId>=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)) 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]); 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) }
else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION)
{ {
b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation); b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation);
} else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY) }
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]); 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 }
else
{ {
b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition); b3CalculateInverseKinematicsAddTargetPurePosition(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition);
} }
if (args.m_flags & B3_HAS_JOINT_DAMPING) if (args.m_flags & B3_HAS_JOINT_DAMPING)
@@ -707,28 +720,23 @@ bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotS
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
int numPos = 0;
int numPos=0;
bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle, bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
&results.m_bodyUniqueId, &results.m_bodyUniqueId,
&numPos, &numPos,
0)!=0; 0) != 0;
if (result && numPos) if (result && numPos)
{ {
results.m_calculatedJointPositions.resize(numPos); results.m_calculatedJointPositions.resize(numPos);
result = b3GetStatusInverseKinematicsJointPositions(statusHandle, result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
&results.m_bodyUniqueId, &results.m_bodyUniqueId,
&numPos, &numPos,
&results.m_calculatedJointPositions[0])!=0; &results.m_calculatedJointPositions[0]) != 0;
} }
return result; 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) 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()) if (!isConnected())
@@ -754,7 +762,7 @@ bool b3RobotSimulatorClientAPI::getLinkState(int bodyUniqueId, int linkIndex, b3
b3Warning("Not connected"); b3Warning("Not connected");
return false; return false;
} }
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId); b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED) if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
@@ -804,30 +812,33 @@ void b3RobotSimulatorClientAPI::getKeyboardEvents(b3KeyboardEventsData* keyboard
b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle); b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle);
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); 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 maxLogDof)
int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds)
{ {
int loggingUniqueId = -1; int loggingUniqueId = -1;
b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryCommandHandle commandHandle;
commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle); commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle);
b3StateLoggingStart(commandHandle,loggingType,fileName.c_str()); b3StateLoggingStart(commandHandle, loggingType, fileName.c_str());
for ( int i=0;i<objectUniqueIds.size();i++) for (int i = 0; i < objectUniqueIds.size(); i++)
{ {
int objectUid = objectUniqueIds[i]; int objectUid = objectUniqueIds[i];
b3StateLoggingAddLoggingObjectUniqueId(commandHandle,objectUid); b3StateLoggingAddLoggingObjectUniqueId(commandHandle, objectUid);
}
if (maxLogDof > 0)
{
b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof);
} }
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
int statusType; int statusType;
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
statusType = b3GetStatusType(statusHandle); statusType = b3GetStatusType(statusHandle);
if (statusType==CMD_STATE_LOGGING_START_COMPLETED) if (statusType == CMD_STATE_LOGGING_START_COMPLETED)
{ {
loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle); loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle);
} }
@@ -842,4 +853,3 @@ void b3RobotSimulatorClientAPI::stopStateLogging(int stateLoggerUniqueId)
b3StateLoggingStop(commandHandle, stateLoggerUniqueId); b3StateLoggingStop(commandHandle, stateLoggerUniqueId);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
} }

56
examples/RobotSimulator/b3RobotSimulatorClientAPI.h
View File

@@ -9,10 +9,6 @@
#include <string> #include <string>
struct b3RobotSimulatorLoadUrdfFileArgs struct b3RobotSimulatorLoadUrdfFileArgs
{ {
b3Vector3 m_startPosition; b3Vector3 m_startPosition;
@@ -21,9 +17,8 @@ struct b3RobotSimulatorLoadUrdfFileArgs
bool m_useMultiBody; bool m_useMultiBody;
b3RobotSimulatorLoadUrdfFileArgs() b3RobotSimulatorLoadUrdfFileArgs()
: : m_startPosition(b3MakeVector3(0, 0, 0)),
m_startPosition(b3MakeVector3(0,0,0)), m_startOrientation(b3Quaternion(0, 0, 0, 1)),
m_startOrientation(b3Quaternion(0,0,0,1)),
m_forceOverrideFixedBase(false), m_forceOverrideFixedBase(false),
m_useMultiBody(true) m_useMultiBody(true)
{ {
@@ -36,14 +31,12 @@ struct b3RobotSimulatorLoadSdfFileArgs
bool m_useMultiBody; bool m_useMultiBody;
b3RobotSimulatorLoadSdfFileArgs() b3RobotSimulatorLoadSdfFileArgs()
: : m_forceOverrideFixedBase(false),
m_forceOverrideFixedBase(false),
m_useMultiBody(true) m_useMultiBody(true)
{ {
} }
}; };
struct b3RobotSimulatorLoadFileResults struct b3RobotSimulatorLoadFileResults
{ {
b3AlignedObjectArray<int> m_uniqueObjectIds; b3AlignedObjectArray<int> m_uniqueObjectIds;
@@ -65,7 +58,7 @@ struct b3RobotSimulatorJointMotorArgs
double m_maxTorqueValue; double m_maxTorqueValue;
b3RobotSimulatorJointMotorArgs(int controlMode) b3RobotSimulatorJointMotorArgs(int controlMode)
:m_controlMode(controlMode), : m_controlMode(controlMode),
m_targetPosition(0), m_targetPosition(0),
m_kp(0.1), m_kp(0.1),
m_targetVelocity(0), m_targetVelocity(0),
@@ -77,16 +70,16 @@ struct b3RobotSimulatorJointMotorArgs
enum b3RobotSimulatorInverseKinematicsFlags enum b3RobotSimulatorInverseKinematicsFlags
{ {
B3_HAS_IK_TARGET_ORIENTATION=1, B3_HAS_IK_TARGET_ORIENTATION = 1,
B3_HAS_NULL_SPACE_VELOCITY=2, B3_HAS_NULL_SPACE_VELOCITY = 2,
B3_HAS_JOINT_DAMPING=4, B3_HAS_JOINT_DAMPING = 4,
}; };
struct b3RobotSimulatorInverseKinematicArgs struct b3RobotSimulatorInverseKinematicArgs
{ {
int m_bodyUniqueId; int m_bodyUniqueId;
// double* m_currentJointPositions; // double* m_currentJointPositions;
// int m_numPositions; // int m_numPositions;
double m_endEffectorTargetPosition[3]; double m_endEffectorTargetPosition[3];
double m_endEffectorTargetOrientation[4]; double m_endEffectorTargetOrientation[4];
int m_endEffectorLinkIndex; int m_endEffectorLinkIndex;
@@ -99,18 +92,18 @@ struct b3RobotSimulatorInverseKinematicArgs
b3AlignedObjectArray<double> m_jointDamping; b3AlignedObjectArray<double> m_jointDamping;
b3RobotSimulatorInverseKinematicArgs() b3RobotSimulatorInverseKinematicArgs()
:m_bodyUniqueId(-1), : m_bodyUniqueId(-1),
m_endEffectorLinkIndex(-1), m_endEffectorLinkIndex(-1),
m_flags(0) m_flags(0)
{ {
m_endEffectorTargetPosition[0]=0; m_endEffectorTargetPosition[0] = 0;
m_endEffectorTargetPosition[1]=0; m_endEffectorTargetPosition[1] = 0;
m_endEffectorTargetPosition[2]=0; m_endEffectorTargetPosition[2] = 0;
m_endEffectorTargetOrientation[0]=0; m_endEffectorTargetOrientation[0] = 0;
m_endEffectorTargetOrientation[1]=0; m_endEffectorTargetOrientation[1] = 0;
m_endEffectorTargetOrientation[2]=0; m_endEffectorTargetOrientation[2] = 0;
m_endEffectorTargetOrientation[3]=1; m_endEffectorTargetOrientation[3] = 1;
} }
}; };
@@ -120,7 +113,6 @@ struct b3RobotSimulatorInverseKinematicsResults
b3AlignedObjectArray<double> m_calculatedJointPositions; b3AlignedObjectArray<double> m_calculatedJointPositions;
}; };
///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet ///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet
///as documented in the pybullet Quickstart Guide ///as documented in the pybullet Quickstart Guide
///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA ///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
@@ -132,7 +124,7 @@ public:
b3RobotSimulatorClientAPI(); b3RobotSimulatorClientAPI();
virtual ~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(); void disconnect();
@@ -145,8 +137,8 @@ public:
b3Quaternion getQuaternionFromEuler(const b3Vector3& rollPitchYaw); b3Quaternion getQuaternionFromEuler(const b3Vector3& rollPitchYaw);
b3Vector3 getEulerFromQuaternion(const b3Quaternion& quat); b3Vector3 getEulerFromQuaternion(const b3Quaternion& quat);
int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args=b3RobotSimulatorLoadUrdfFileArgs()); 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 loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results = b3RobotSimulatorLoadFileResults(), const struct b3RobotSimulatorLoadSdfFileArgs& args = b3RobotSimulatorLoadSdfFileArgs());
bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
@@ -155,13 +147,16 @@ public:
bool getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const; bool getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const;
bool resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation); 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; int getNumJoints(int bodyUniqueId) const;
bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo); bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo);
void createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo); void createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo);
bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState *state); bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state);
bool resetJointState(int bodyUniqueId, int jointIndex, double targetValue); bool resetJointState(int bodyUniqueId, int jointIndex, double targetValue);
@@ -187,12 +182,11 @@ public:
void configureDebugVisualizer(enum b3ConfigureDebugVisualizerEnum flag, int enable); 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 stopStateLogging(int stateLoggerUniqueId);
void getVREvents(b3VREventsData* vrEventsData); void getVREvents(b3VREventsData* vrEventsData);
void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData); void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData);
}; };
#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H #endif //B3_ROBOT_SIMULATOR_CLIENT_API_H

32
src/Bullet3Common/b3Quaternion.h
View File

@@ -126,15 +126,16 @@ public:
sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
} }
/**@brief Set the quaternion using euler angles /**@brief Set the quaternion using euler angles
* @param yaw Angle around Z * @param yaw Angle around Z
* @param pitch Angle around Y * @param pitch Angle around Y
* @param roll Angle around X */ * @param roll Angle around X */
void setEulerZYX(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll) void setEulerZYX(const b3Scalar& yawZ, const b3Scalar& pitchY, const b3Scalar& rollX)
{ {
b3Scalar halfYaw = b3Scalar(yaw) * b3Scalar(0.5); b3Scalar halfYaw = b3Scalar(yawZ) * b3Scalar(0.5);
b3Scalar halfPitch = b3Scalar(pitch) * b3Scalar(0.5); b3Scalar halfPitch = b3Scalar(pitchY) * b3Scalar(0.5);
b3Scalar halfRoll = b3Scalar(roll) * b3Scalar(0.5); b3Scalar halfRoll = b3Scalar(rollX) * b3Scalar(0.5);
b3Scalar cosYaw = b3Cos(halfYaw); b3Scalar cosYaw = b3Cos(halfYaw);
b3Scalar sinYaw = b3Sin(halfYaw); b3Scalar sinYaw = b3Sin(halfYaw);
b3Scalar cosPitch = b3Cos(halfPitch); b3Scalar cosPitch = b3Cos(halfPitch);
@@ -145,7 +146,30 @@ public:
cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
normalize();
} }
/**@brief Get the euler angles from this quaternion
* @param yaw Angle around Z
* @param pitch Angle around Y
* @param roll Angle around X */
void getEulerZYX(b3Scalar& yawZ, b3Scalar& pitchY, b3Scalar& rollX) const
{
b3Scalar squ;
b3Scalar sqx;
b3Scalar sqy;
b3Scalar sqz;
b3Scalar sarg;
sqx = m_floats[0] * m_floats[0];
sqy = m_floats[1] * m_floats[1];
sqz = m_floats[2] * m_floats[2];
squ = m_floats[3] * m_floats[3];
rollX = b3Atan2(2 * (m_floats[1] * m_floats[2] + m_floats[3] * m_floats[0]), squ - sqx - sqy + sqz);
sarg = b3Scalar(-2.) * (m_floats[0] * m_floats[2] - m_floats[3] * m_floats[1]);
pitchY = sarg <= b3Scalar(-1.0) ? b3Scalar(-0.5) * B3_PI: (sarg >= b3Scalar(1.0) ? b3Scalar(0.5) * B3_PI : b3Asin(sarg));
yawZ = b3Atan2(2 * (m_floats[0] * m_floats[1] + m_floats[3] * m_floats[2]), squ + sqx - sqy - sqz);
}
/**@brief Add two quaternions /**@brief Add two quaternions
* @param q The quaternion to add to this one */ * @param q The quaternion to add to this one */
B3_FORCE_INLINE b3Quaternion& operator+=(const b3Quaternion& q) B3_FORCE_INLINE b3Quaternion& operator+=(const b3Quaternion& q)