diff --git a/examples/RobotSimulator/MinitaurSetup.cpp b/examples/RobotSimulator/MinitaurSetup.cpp index 0d1238c87..e867d0583 100644 --- a/examples/RobotSimulator/MinitaurSetup.cpp +++ b/examples/RobotSimulator/MinitaurSetup.cpp @@ -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;isyncBodies(); - - 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"); diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp index 224014b76..4c95006d2 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp @@ -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& objectUniqueIds) +int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray& 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 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); } - diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h index e6f658cc5..9fe1b8c50 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h @@ -9,23 +9,18 @@ #include - - - - 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 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 m_lowerLimits; - b3AlignedObjectArray m_upperLimits; - b3AlignedObjectArray m_jointRanges; - b3AlignedObjectArray m_restPoses; - b3AlignedObjectArray m_jointDamping; + int m_numDegreeOfFreedom; + b3AlignedObjectArray m_lowerLimits; + b3AlignedObjectArray m_upperLimits; + b3AlignedObjectArray m_jointRanges; + b3AlignedObjectArray m_restPoses; + b3AlignedObjectArray 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 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& objectUniqueIds); + int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray& 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 diff --git a/src/Bullet3Common/b3Quaternion.h b/src/Bullet3Common/b3Quaternion.h index c89f2cf39..0b7564dcd 100644 --- a/src/Bullet3Common/b3Quaternion.h +++ b/src/Bullet3Common/b3Quaternion.h @@ -126,15 +126,16 @@ public: sinRoll * cosPitch * cosYaw - cosRoll * 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 pitch Angle around Y * @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 halfPitch = b3Scalar(pitch) * b3Scalar(0.5); - b3Scalar halfRoll = b3Scalar(roll) * b3Scalar(0.5); + b3Scalar halfYaw = b3Scalar(yawZ) * b3Scalar(0.5); + b3Scalar halfPitch = b3Scalar(pitchY) * b3Scalar(0.5); + b3Scalar halfRoll = b3Scalar(rollX) * b3Scalar(0.5); b3Scalar cosYaw = b3Cos(halfYaw); b3Scalar sinYaw = b3Sin(halfYaw); b3Scalar cosPitch = b3Cos(halfPitch); @@ -145,7 +146,30 @@ public: cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z 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 * @param q The quaternion to add to this one */ B3_FORCE_INLINE b3Quaternion& operator+=(const b3Quaternion& q)