From 57b3e0d221687bc3f908a265b8d1308460cb784c Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 27 May 2018 10:42:33 +1000 Subject: [PATCH 1/2] Move from b3Vector3 to btVector3 to support double precision in examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI API. --- examples/RobotSimulator/MinitaurSetup.cpp | 14 +- examples/RobotSimulator/MinitaurSetup.h | 6 +- .../RobotSimulator/RobotSimulatorMain.cpp | 14 +- .../b3RobotSimulatorClientAPI_NoGUI.cpp | 148 +++++++++--------- .../b3RobotSimulatorClientAPI_NoGUI.h | 94 +++++------ .../RoboticsLearning/GripperGraspExample.cpp | 10 +- .../RoboticsLearning/KukaGraspExample.cpp | 2 +- .../RoboticsLearning/R2D2GraspExample.cpp | 6 +- 8 files changed, 147 insertions(+), 147 deletions(-) diff --git a/examples/RobotSimulator/MinitaurSetup.cpp b/examples/RobotSimulator/MinitaurSetup.cpp index 16d102072..ece03d66d 100644 --- a/examples/RobotSimulator/MinitaurSetup.cpp +++ b/examples/RobotSimulator/MinitaurSetup.cpp @@ -85,8 +85,8 @@ static const char* minitaurURDF="quadruped/minitaur_rainbow_dash_v1.urdf"; "motor_back_leftL_bracket_joint", }; - static b3Vector3 KNEE_CONSTRAINT_POINT_LONG = b3MakeVector3(0, 0.0045, 0.088); - static b3Vector3 KNEE_CONSTRAINT_POINT_SHORT= b3MakeVector3(0, 0.0045, 0.100); + static btVector3 KNEE_CONSTRAINT_POINT_LONG = btVector3(0, 0.0045, 0.088); + static btVector3 KNEE_CONSTRAINT_POINT_SHORT= btVector3(0, 0.0045, 0.100); #elif defined(MINITAUR_RAINBOWDASH_V0) static const char* minitaurURDF="quadruped/minitaur_rainbow_dash.urdf"; @@ -119,8 +119,8 @@ static const char* minitaurURDF="quadruped/minitaur_rainbow_dash_v1.urdf"; "motor_front_rightR_joint",//14 "knee_front_rightR_joint",//15 }; - static b3Vector3 KNEE_CONSTRAINT_POINT_LONG = b3MakeVector3(0, 0.0045, 0.088); - static b3Vector3 KNEE_CONSTRAINT_POINT_SHORT= b3MakeVector3(0, 0.0045, 0.100); + static btVector3 KNEE_CONSTRAINT_POINT_LONG = btVector3(0, 0.0045, 0.088); + static btVector3 KNEE_CONSTRAINT_POINT_SHORT= btVector3(0, 0.0045, 0.100); #elif defined(MINITAUR_V0) static const char* minitaurURDF="quadruped/minitaur.urdf"; @@ -153,8 +153,8 @@ static const char* minitaurURDF="quadruped/minitaur_rainbow_dash_v1.urdf"; "motor_front_rightR_joint", "knee_front_rightR_link", }; - static b3Vector3 KNEE_CONSTRAINT_POINT_LONG = b3MakeVector3(0, 0.005, 0.2); - static b3Vector3 KNEE_CONSTRAINT_POINT_SHORT= b3MakeVector3(0, 0.01, 0.2); + static btVector3 KNEE_CONSTRAINT_POINT_LONG = btVector3(0, 0.005, 0.2); + static btVector3 KNEE_CONSTRAINT_POINT_SHORT= btVector3(0, 0.01, 0.2); #endif @@ -255,7 +255,7 @@ void MinitaurSetup::resetPose(class b3RobotSimulatorClientAPI_NoGUI* sim) } -int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI_NoGUI* sim, const b3Vector3& startPos, const b3Quaternion& startOrn) +int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI_NoGUI* sim, const btVector3& startPos, const btQuaternion& startOrn) { b3RobotSimulatorLoadUrdfFileArgs args; diff --git a/examples/RobotSimulator/MinitaurSetup.h b/examples/RobotSimulator/MinitaurSetup.h index 6d9f8c211..83445eb2c 100644 --- a/examples/RobotSimulator/MinitaurSetup.h +++ b/examples/RobotSimulator/MinitaurSetup.h @@ -1,8 +1,8 @@ #ifndef MINITAUR_SIMULATION_SETUP_H #define MINITAUR_SIMULATION_SETUP_H -#include "Bullet3Common/b3Vector3.h" -#include "Bullet3Common/b3Quaternion.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btQuaternion.h" class MinitaurSetup { struct MinitaurSetupInternalData* m_data; @@ -12,7 +12,7 @@ public: MinitaurSetup(); virtual ~MinitaurSetup(); - int setupMinitaur(class b3RobotSimulatorClientAPI_NoGUI* sim, const class b3Vector3& startPos=b3MakeVector3(0,0,0), const class b3Quaternion& startOrn = b3Quaternion(0,0,0,1)); + int setupMinitaur(class b3RobotSimulatorClientAPI_NoGUI* sim, const class btVector3& startPos=btVector3(0,0,0), const class btQuaternion& startOrn = btQuaternion(0,0,0,1)); void setDesiredMotorAngle(class b3RobotSimulatorClientAPI_NoGUI* sim, const char* motorName, double desiredAngle, double maxTorque=3,double kp=0.1, double kd=0.9); diff --git a/examples/RobotSimulator/RobotSimulatorMain.cpp b/examples/RobotSimulator/RobotSimulatorMain.cpp index 2a6b90584..c19933735 100644 --- a/examples/RobotSimulator/RobotSimulatorMain.cpp +++ b/examples/RobotSimulator/RobotSimulatorMain.cpp @@ -37,15 +37,15 @@ int main(int argc, char* argv[]) sim->setTimeOut(10); //syncBodies is only needed when connecting to an existing physics server that has already some bodies sim->syncBodies(); - b3Scalar fixedTimeStep = 1./240.; + btScalar fixedTimeStep = 1./240.; sim->setTimeStep(fixedTimeStep); - b3Quaternion q = sim->getQuaternionFromEuler(b3MakeVector3(0.1,0.2,0.3)); - b3Vector3 rpy; + btQuaternion q = sim->getQuaternionFromEuler(btVector3(0.1,0.2,0.3)); + btVector3 rpy; rpy = sim->getEulerFromQuaternion(q); - sim->setGravity(b3MakeVector3(0,0,-9.8)); + sim->setGravity(btVector3(0,0,-9.8)); //int blockId = sim->loadURDF("cube.urdf"); //b3BodyInfo bodyInfo; @@ -54,7 +54,7 @@ int main(int argc, char* argv[]) sim->loadURDF("plane.urdf"); MinitaurSetup minitaur; - int minitaurUid = minitaur.setupMinitaur(sim, b3MakeVector3(0,0,.3)); + int minitaurUid = minitaur.setupMinitaur(sim, btVector3(0,0,.3)); //b3RobotSimulatorLoadUrdfFileArgs args; @@ -139,8 +139,8 @@ int main(int argc, char* argv[]) static double yaw=0; double distance = 1; yaw+=0.1; - b3Vector3 basePos; - b3Quaternion baseOrn; + btVector3 basePos; + btQuaternion baseOrn; sim->getBasePositionAndOrientation(minitaurUid,basePos,baseOrn); sim->resetDebugVisualizerCamera(distance,-20, yaw,basePos); } diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp index 49b8524da..cc32a3913 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp @@ -17,7 +17,7 @@ #include "../SharedMemory/SharedMemoryPublic.h" #include "Bullet3Common/b3Logging.h" -static void scalarToDouble3(b3Scalar a[3], double b[3]) +static void scalarToDouble3(btScalar a[3], double b[3]) { for (int i = 0; i < 3; i++) { @@ -25,7 +25,7 @@ static void scalarToDouble3(b3Scalar a[3], double b[3]) } } -static void scalarToDouble4(b3Scalar a[4], double b[4]) +static void scalarToDouble4(btScalar a[4], double b[4]) { for (int i = 0; i < 4; i++) { @@ -206,38 +206,38 @@ void b3RobotSimulatorClientAPI_NoGUI::stepSimulation() { statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle)); statusType = b3GetStatusType(statusHandle); - //b3Assert(statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED); + //btAssert(statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED); } } -void b3RobotSimulatorClientAPI_NoGUI::setGravity(const b3Vector3& gravityAcceleration) +void b3RobotSimulatorClientAPI_NoGUI::setGravity(const btVector3& gravityAcceleration) { if (!isConnected()) { b3Warning("Not connected"); return; } - b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle)); + btAssert(b3CanSubmitCommand(m_data->m_physicsClientHandle)); 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); + // btAssert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); } -b3Quaternion b3RobotSimulatorClientAPI_NoGUI::getQuaternionFromEuler(const b3Vector3& rollPitchYaw) +btQuaternion b3RobotSimulatorClientAPI_NoGUI::getQuaternionFromEuler(const btVector3& rollPitchYaw) { - b3Quaternion q; + btQuaternion q; q.setEulerZYX(rollPitchYaw[2],rollPitchYaw[1],rollPitchYaw[0]); return q; } -b3Vector3 b3RobotSimulatorClientAPI_NoGUI::getEulerFromQuaternion(const b3Quaternion& quat) +btVector3 b3RobotSimulatorClientAPI_NoGUI::getEulerFromQuaternion(const btQuaternion& quat) { - b3Scalar roll,pitch,yaw; + btScalar roll,pitch,yaw; quat.getEulerZYX(yaw,pitch,roll); - b3Vector3 rpy2 = b3MakeVector3(roll,pitch,yaw); + btVector3 rpy2 = btVector3(roll,pitch,yaw); return rpy2; } @@ -271,7 +271,7 @@ int b3RobotSimulatorClientAPI_NoGUI::loadURDF(const std::string& fileName, const statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusType = b3GetStatusType(statusHandle); - b3Assert(statusType == CMD_URDF_LOADING_COMPLETED); + btAssert(statusType == CMD_URDF_LOADING_COMPLETED); if (statusType == CMD_URDF_LOADING_COMPLETED) { robotUniqueId = b3GetStatusBodyIndex(statusHandle); @@ -356,7 +356,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::loadSDF(const std::string& fileName, b3Rob b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusType = b3GetStatusType(statusHandle); - b3Assert(statusType == CMD_SDF_LOADING_COMPLETED); + btAssert(statusType == CMD_SDF_LOADING_COMPLETED); if (statusType == CMD_SDF_LOADING_COMPLETED) { int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0); @@ -384,7 +384,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getBodyInfo(int bodyUniqueId, struct b3Bod return (result != 0); } -bool b3RobotSimulatorClientAPI_NoGUI::getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const +bool b3RobotSimulatorClientAPI_NoGUI::getBasePositionAndOrientation(int bodyUniqueId, btVector3& basePosition, btQuaternion& baseOrientation) const { if (!isConnected()) { @@ -422,7 +422,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getBasePositionAndOrientation(int bodyUniq return true; } -bool b3RobotSimulatorClientAPI_NoGUI::resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) +bool b3RobotSimulatorClientAPI_NoGUI::resetBasePositionAndOrientation(int bodyUniqueId, btVector3& basePosition, btQuaternion& baseOrientation) { if (!isConnected()) { @@ -444,7 +444,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::resetBasePositionAndOrientation(int bodyUn return true; } -bool b3RobotSimulatorClientAPI_NoGUI::getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const +bool b3RobotSimulatorClientAPI_NoGUI::getBaseVelocity(int bodyUniqueId, btVector3& baseLinearVelocity, btVector3& baseAngularVelocity) const { if (!isConnected()) { @@ -478,7 +478,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getBaseVelocity(int bodyUniqueId, b3Vector return true; } -bool b3RobotSimulatorClientAPI_NoGUI::resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const +bool b3RobotSimulatorClientAPI_NoGUI::resetBaseVelocity(int bodyUniqueId, const btVector3& linearVelocity, const btVector3& angularVelocity) const { if (!isConnected()) { @@ -491,11 +491,11 @@ bool b3RobotSimulatorClientAPI_NoGUI::resetBaseVelocity(int bodyUniqueId, const commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); - b3Vector3DoubleData linVelDouble; + btVector3DoubleData linVelDouble; linearVelocity.serializeDouble(linVelDouble); b3CreatePoseCommandSetBaseLinearVelocity(commandHandle, linVelDouble.m_floats); - b3Vector3DoubleData angVelDouble; + btVector3DoubleData angVelDouble; angularVelocity.serializeDouble(angVelDouble); b3CreatePoseCommandSetBaseAngularVelocity(commandHandle, angVelDouble.m_floats); @@ -534,7 +534,7 @@ void b3RobotSimulatorClientAPI_NoGUI::setRealTimeSimulation(bool enableRealTimeS statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); + btAssert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); } int b3RobotSimulatorClientAPI_NoGUI::getNumJoints(int bodyUniqueId) const @@ -565,7 +565,7 @@ int b3RobotSimulatorClientAPI_NoGUI::createConstraint(int parentBodyIndex, int p return -1; } b3SharedMemoryStatusHandle statusHandle; - b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle)); + btAssert(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)); @@ -792,7 +792,7 @@ void b3RobotSimulatorClientAPI_NoGUI::setNumSolverIterations(int numIterations) b3SharedMemoryStatusHandle statusHandle; b3PhysicsParamSetNumSolverIterations(command, numIterations); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); + btAssert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); } void b3RobotSimulatorClientAPI_NoGUI::setContactBreakingThreshold(double threshold) @@ -806,7 +806,7 @@ void b3RobotSimulatorClientAPI_NoGUI::setContactBreakingThreshold(double thresho b3SharedMemoryStatusHandle statusHandle; b3PhysicsParamSetContactBreakingThreshold(command,threshold); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); + btAssert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); } @@ -836,7 +836,7 @@ void b3RobotSimulatorClientAPI_NoGUI::setNumSimulationSubSteps(int numSubSteps) b3SharedMemoryStatusHandle statusHandle; b3PhysicsParamSetNumSubSteps(command, numSubSteps); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); + btAssert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); } bool b3RobotSimulatorClientAPI_NoGUI::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results) @@ -846,8 +846,8 @@ bool b3RobotSimulatorClientAPI_NoGUI::calculateInverseKinematics(const struct b3 b3Warning("Not connected"); return false; } - b3Assert(args.m_endEffectorLinkIndex >= 0); - b3Assert(args.m_bodyUniqueId >= 0); + btAssert(args.m_endEffectorLinkIndex >= 0); + btAssert(args.m_bodyUniqueId >= 0); 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)) @@ -989,7 +989,7 @@ void b3RobotSimulatorClientAPI_NoGUI::getKeyboardEvents(b3KeyboardEventsData* ke b3GetKeyboardEventsData(m_data->m_physicsClientHandle, keyboardEventsData); } -int b3RobotSimulatorClientAPI_NoGUI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray& objectUniqueIds, int maxLogDof) +int b3RobotSimulatorClientAPI_NoGUI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const btAlignedObjectArray& objectUniqueIds, int maxLogDof) { if (!isConnected()) { @@ -1039,7 +1039,7 @@ void b3RobotSimulatorClientAPI_NoGUI::stopStateLogging(int stateLoggerUniqueId) statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); } -void b3RobotSimulatorClientAPI_NoGUI::resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const b3Vector3& targetPos) +void b3RobotSimulatorClientAPI_NoGUI::resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const btVector3& targetPos) { if (!isConnected()) { @@ -1052,7 +1052,7 @@ void b3RobotSimulatorClientAPI_NoGUI::resetDebugVisualizerCamera(double cameraDi { if ((cameraDistance >= 0)) { - b3Vector3FloatData camTargetPos; + btVector3FloatData camTargetPos; targetPos.serializeFloat(camTargetPos); b3ConfigureOpenGLVisualizerSetViewMatrix(commandHandle, cameraDistance, cameraPitch, cameraYaw, camTargetPos.m_floats); } @@ -1428,12 +1428,12 @@ int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, double *posXYZ return -1; } -int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, b3Vector3 &posXYZ, struct b3RobotSimulatorAddUserDebugTextArgs & args) +int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, btVector3 &posXYZ, struct b3RobotSimulatorAddUserDebugTextArgs & args) { double dposXYZ[3]; - dposXYZ[0] = posXYZ.x; - dposXYZ[1] = posXYZ.y; - dposXYZ[2] = posXYZ.z; + dposXYZ[0] = posXYZ.x(); + dposXYZ[1] = posXYZ.y(); + dposXYZ[2] = posXYZ.z(); return addUserDebugText(text, &dposXYZ[0], args); } @@ -1466,17 +1466,17 @@ int b3RobotSimulatorClientAPI_NoGUI::addUserDebugLine(double *fromXYZ, double *t return -1; } -int b3RobotSimulatorClientAPI_NoGUI::addUserDebugLine(b3Vector3 &fromXYZ, b3Vector3 &toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args) +int b3RobotSimulatorClientAPI_NoGUI::addUserDebugLine(btVector3 &fromXYZ, btVector3 &toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args) { double dfromXYZ[3]; double dtoXYZ[3]; - dfromXYZ[0] = fromXYZ.x; - dfromXYZ[1] = fromXYZ.y; - dfromXYZ[2] = fromXYZ.z; + dfromXYZ[0] = fromXYZ.x(); + dfromXYZ[1] = fromXYZ.y(); + dfromXYZ[2] = fromXYZ.z(); - dtoXYZ[0] = toXYZ.x; - dtoXYZ[1] = toXYZ.y; - dtoXYZ[2] = toXYZ.z; + dtoXYZ[0] = toXYZ.x(); + dtoXYZ[1] = toXYZ.y(); + dtoXYZ[2] = toXYZ.z(); return addUserDebugLine(&dfromXYZ[0], &dtoXYZ[0], args); } @@ -1647,18 +1647,18 @@ bool b3RobotSimulatorClientAPI_NoGUI::applyExternalForce(int objectUniqueId, int return true; } -bool b3RobotSimulatorClientAPI_NoGUI::applyExternalForce(int objectUniqueId, int linkIndex, b3Vector3 &force, b3Vector3 &position, int flags) +bool b3RobotSimulatorClientAPI_NoGUI::applyExternalForce(int objectUniqueId, int linkIndex, btVector3 &force, btVector3 &position, int flags) { double dforce[3]; double dposition[3]; - dforce[0] = force.x; - dforce[1] = force.y; - dforce[2] = force.z; + dforce[0] = force.x(); + dforce[1] = force.y(); + dforce[2] = force.z(); - dposition[0] = position.x; - dposition[1] = position.y; - dposition[2] = position.z; + dposition[0] = position.x(); + dposition[1] = position.y(); + dposition[2] = position.z(); return applyExternalForce(objectUniqueId, linkIndex, &dforce[0], &dposition[0], flags); } @@ -1680,13 +1680,13 @@ bool b3RobotSimulatorClientAPI_NoGUI::applyExternalTorque(int objectUniqueId, in return true; } -bool b3RobotSimulatorClientAPI_NoGUI::applyExternalTorque(int objectUniqueId, int linkIndex, b3Vector3 &torque, int flags) +bool b3RobotSimulatorClientAPI_NoGUI::applyExternalTorque(int objectUniqueId, int linkIndex, btVector3 &torque, int flags) { double dtorque[3]; - dtorque[0] = torque.x; - dtorque[1] = torque.y; - dtorque[2] = torque.z; + dtorque[0] = torque.x(); + dtorque[1] = torque.y(); + dtorque[2] = torque.z(); return applyExternalTorque(objectUniqueId, linkIndex, &dtorque[0], flags); } @@ -1829,18 +1829,18 @@ bool b3RobotSimulatorClientAPI_NoGUI::getOverlappingObjects(double *aabbMin, dou } -bool b3RobotSimulatorClientAPI_NoGUI::getOverlappingObjects(b3Vector3 &aabbMin, b3Vector3 &aabbMax, struct b3AABBOverlapData *overlapData) +bool b3RobotSimulatorClientAPI_NoGUI::getOverlappingObjects(btVector3 &aabbMin, btVector3 &aabbMax, struct b3AABBOverlapData *overlapData) { double daabbMin[3]; double daabbMax[3]; - daabbMin[0] = aabbMin.x; - daabbMin[1] = aabbMin.y; - daabbMin[2] = aabbMin.z; + daabbMin[0] = aabbMin.x(); + daabbMin[1] = aabbMin.y(); + daabbMin[2] = aabbMin.z(); - daabbMax[0] = aabbMax.x; - daabbMax[1] = aabbMax.y; - daabbMax[2] = aabbMax.z; + daabbMax[0] = aabbMax.x(); + daabbMax[1] = aabbMax.y(); + daabbMax[2] = aabbMax.z(); return getOverlappingObjects(&daabbMin[0], &daabbMax[0], overlapData); } @@ -1886,20 +1886,20 @@ bool b3RobotSimulatorClientAPI_NoGUI::getAABB(int bodyUniqueId, int linkIndex, d return false; } -bool b3RobotSimulatorClientAPI_NoGUI::getAABB(int bodyUniqueId, int linkIndex, b3Vector3 &aabbMin, b3Vector3 &aabbMax) +bool b3RobotSimulatorClientAPI_NoGUI::getAABB(int bodyUniqueId, int linkIndex, btVector3 &aabbMin, btVector3 &aabbMax) { double daabbMin[3]; double daabbMax[3]; bool status = getAABB(bodyUniqueId, linkIndex, &daabbMin[0], &daabbMax[0]); - aabbMin.x = (float)daabbMin[0]; - aabbMin.y = (float)daabbMin[1]; - aabbMin.z = (float)daabbMin[2]; + aabbMin[0] = (float)daabbMin[0]; + aabbMin[1] = (float)daabbMin[1]; + aabbMin[2] = (float)daabbMin[2]; - aabbMax.x = (float)daabbMax[0]; - aabbMax.y = (float)daabbMax[1]; - aabbMax.z = (float)daabbMax[2]; + aabbMax[0] = (float)daabbMax[0]; + aabbMax[1] = (float)daabbMax[1]; + aabbMax[2] = (float)daabbMax[2]; return status; } @@ -1975,8 +1975,8 @@ int b3RobotSimulatorClientAPI_NoGUI::createMultiBody(struct b3RobotSimulatorCrea double doubleBaseOrientation[4]; double doubleBaseInertialFrameOrientation[4]; - scalarToDouble4(args.m_baseOrientation.m_floats, doubleBaseOrientation); - scalarToDouble4(args.m_baseInertialFrameOrientation.m_floats, doubleBaseInertialFrameOrientation); + scalarToDouble4(args.m_baseOrientation, doubleBaseOrientation); + scalarToDouble4(args.m_baseInertialFrameOrientation, doubleBaseInertialFrameOrientation); command = b3CreateMultiBodyCommandInit(sm); @@ -1987,13 +1987,13 @@ int b3RobotSimulatorClientAPI_NoGUI::createMultiBody(struct b3RobotSimulatorCrea double linkMass = args.m_linkMasses[i]; int linkCollisionShapeIndex = args.m_linkCollisionShapeIndices[i]; int linkVisualShapeIndex = args.m_linkVisualShapeIndices[i]; - b3Vector3 linkPosition = args.m_linkPositions[i]; - b3Quaternion linkOrientation = args.m_linkOrientations[i]; - b3Vector3 linkInertialFramePosition = args.m_linkInertialFramePositions[i]; - b3Quaternion linkInertialFrameOrientation = args.m_linkInertialFrameOrientations[i]; + btVector3 linkPosition = args.m_linkPositions[i]; + btQuaternion linkOrientation = args.m_linkOrientations[i]; + btVector3 linkInertialFramePosition = args.m_linkInertialFramePositions[i]; + btQuaternion linkInertialFrameOrientation = args.m_linkInertialFrameOrientations[i]; int linkParentIndex = args.m_linkParentIndices[i]; int linkJointType = args.m_linkJointTypes[i]; - b3Vector3 linkJointAxis = args.m_linkJointAxes[i]; + btVector3 linkJointAxis = args.m_linkJointAxes[i]; double doubleLinkPosition[3]; double doubleLinkInertialFramePosition[3]; @@ -2004,8 +2004,8 @@ int b3RobotSimulatorClientAPI_NoGUI::createMultiBody(struct b3RobotSimulatorCrea double doubleLinkOrientation[4]; double doubleLinkInertialFrameOrientation[4]; - scalarToDouble4(linkOrientation.m_floats, doubleLinkOrientation); - scalarToDouble4(linkInertialFrameOrientation.m_floats, doubleLinkInertialFrameOrientation); + scalarToDouble4(linkOrientation, doubleLinkOrientation); + scalarToDouble4(linkInertialFrameOrientation, doubleLinkInertialFrameOrientation); b3CreateMultiBodyLink(command, linkMass, diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h index 52c9f394f..02594cf40 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h @@ -2,22 +2,22 @@ #define B3_ROBOT_SIMULATOR_CLIENT_API_H #include "SharedMemoryPublic.h" -#include "Bullet3Common/b3Vector3.h" -#include "Bullet3Common/b3Quaternion.h" -#include "Bullet3Common/b3Transform.h" -#include "Bullet3Common/b3AlignedObjectArray.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btQuaternion.h" +#include "LinearMath/btTransform.h" +#include "LinearMath/btAlignedObjectArray.h" #include struct b3RobotSimulatorLoadUrdfFileArgs { - b3Vector3 m_startPosition; - b3Quaternion m_startOrientation; + btVector3 m_startPosition; + btQuaternion m_startOrientation; bool m_forceOverrideFixedBase; bool m_useMultiBody; int m_flags; - b3RobotSimulatorLoadUrdfFileArgs(const b3Vector3& startPos, const b3Quaternion& startOrn) + b3RobotSimulatorLoadUrdfFileArgs(const btVector3& startPos, const btQuaternion& startOrn) : m_startPosition(startPos), m_startOrientation(startOrn), m_forceOverrideFixedBase(false), @@ -27,8 +27,8 @@ struct b3RobotSimulatorLoadUrdfFileArgs } b3RobotSimulatorLoadUrdfFileArgs() - : m_startPosition(b3MakeVector3(0, 0, 0)), - m_startOrientation(b3Quaternion(0, 0, 0, 1)), + : m_startPosition(btVector3(0, 0, 0)), + m_startOrientation(btQuaternion(0, 0, 0, 1)), m_forceOverrideFixedBase(false), m_useMultiBody(true), m_flags(0) @@ -50,7 +50,7 @@ struct b3RobotSimulatorLoadSdfFileArgs struct b3RobotSimulatorLoadFileResults { - b3AlignedObjectArray m_uniqueObjectIds; + btAlignedObjectArray m_uniqueObjectIds; b3RobotSimulatorLoadFileResults() { } @@ -96,11 +96,11 @@ struct b3RobotSimulatorInverseKinematicArgs int m_endEffectorLinkIndex; int m_flags; int m_numDegreeOfFreedom; - b3AlignedObjectArray m_lowerLimits; - b3AlignedObjectArray m_upperLimits; - b3AlignedObjectArray m_jointRanges; - b3AlignedObjectArray m_restPoses; - b3AlignedObjectArray m_jointDamping; + btAlignedObjectArray m_lowerLimits; + btAlignedObjectArray m_upperLimits; + btAlignedObjectArray m_jointRanges; + btAlignedObjectArray m_restPoses; + btAlignedObjectArray m_jointDamping; b3RobotSimulatorInverseKinematicArgs() : m_bodyUniqueId(-1), @@ -121,7 +121,7 @@ struct b3RobotSimulatorInverseKinematicArgs struct b3RobotSimulatorInverseKinematicsResults { int m_bodyUniqueId; - b3AlignedObjectArray m_calculatedJointPositions; + btAlignedObjectArray m_calculatedJointPositions; }; struct b3JointStates2 @@ -129,10 +129,10 @@ struct b3JointStates2 int m_bodyUniqueId; int m_numDegreeOfFreedomQ; int m_numDegreeOfFreedomU; - b3Transform m_rootLocalInertialFrame; - b3AlignedObjectArray m_actualStateQ; - b3AlignedObjectArray m_actualStateQdot; - b3AlignedObjectArray m_jointReactionForces; + btTransform m_rootLocalInertialFrame; + btAlignedObjectArray m_actualStateQ; + btAlignedObjectArray m_actualStateQdot; + btAlignedObjectArray m_jointReactionForces; }; @@ -322,11 +322,11 @@ struct b3RobotSimulatorCreateCollisionShapeArgs { int m_shapeType; double m_radius; - b3Vector3 m_halfExtents; + btVector3 m_halfExtents; double m_height; char* m_fileName; - b3Vector3 m_meshScale; - b3Vector3 m_planeNormal; + btVector3 m_meshScale; + btVector3 m_planeNormal; int m_flags; b3RobotSimulatorCreateCollisionShapeArgs() : m_shapeType(-1), @@ -355,22 +355,22 @@ struct b3RobotSimulatorCreateMultiBodyArgs double m_baseMass; int m_baseCollisionShapeIndex; int m_baseVisualShapeIndex; - b3Vector3 m_basePosition; - b3Quaternion m_baseOrientation; - b3Vector3 m_baseInertialFramePosition; - b3Quaternion m_baseInertialFrameOrientation; + btVector3 m_basePosition; + btQuaternion m_baseOrientation; + btVector3 m_baseInertialFramePosition; + btQuaternion m_baseInertialFrameOrientation; int m_numLinks; double *m_linkMasses; int *m_linkCollisionShapeIndices; int *m_linkVisualShapeIndices; - b3Vector3 *m_linkPositions; - b3Quaternion *m_linkOrientations; - b3Vector3 *m_linkInertialFramePositions; - b3Quaternion *m_linkInertialFrameOrientations; + btVector3 *m_linkPositions; + btQuaternion *m_linkOrientations; + btVector3 *m_linkInertialFramePositions; + btQuaternion *m_linkInertialFrameOrientations; int *m_linkParentIndices; int *m_linkJointTypes; - b3Vector3 *m_linkJointAxes; + btVector3 *m_linkJointAxes; int m_useMaximalCoordinates; @@ -422,8 +422,8 @@ public: void resetSimulation(); - b3Quaternion getQuaternionFromEuler(const b3Vector3& rollPitchYaw); - b3Vector3 getEulerFromQuaternion(const b3Quaternion& quat); + btQuaternion getQuaternionFromEuler(const btVector3& rollPitchYaw); + btVector3 getEulerFromQuaternion(const btQuaternion& quat); int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args = b3RobotSimulatorLoadUrdfFileArgs()); bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args = b3RobotSimulatorLoadSdfFileArgs()); @@ -432,11 +432,11 @@ public: bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo); - bool getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const; - bool resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation); + bool getBasePositionAndOrientation(int bodyUniqueId, btVector3& basePosition, btQuaternion& baseOrientation) const; + bool resetBasePositionAndOrientation(int bodyUniqueId, btVector3& basePosition, btQuaternion& baseOrientation); - bool getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const; - bool resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const; + bool getBaseVelocity(int bodyUniqueId, btVector3& baseLinearVelocity, btVector3& baseAngularVelocity) const; + bool resetBaseVelocity(int bodyUniqueId, const btVector3& linearVelocity, const btVector3& angularVelocity) const; int getNumJoints(int bodyUniqueId) const; @@ -468,7 +468,7 @@ public: void setInternalSimFlags(int flags); - void setGravity(const b3Vector3& gravityAcceleration); + void setGravity(const btVector3& gravityAcceleration); void setTimeStep(double timeStepInSeconds); void setNumSimulationSubSteps(int numSubSteps); @@ -482,9 +482,9 @@ public: bool getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState); void configureDebugVisualizer(enum b3ConfigureDebugVisualizerEnum flag, int enable); - void resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const b3Vector3& targetPos); + void resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const btVector3& targetPos); - int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray& objectUniqueIds=b3AlignedObjectArray(), int maxLogDof = -1); + int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const btAlignedObjectArray& objectUniqueIds=btAlignedObjectArray(), int maxLogDof = -1); void stopStateLogging(int stateLoggerUniqueId); void getVREvents(b3VREventsData* vrEventsData, int deviceTypeFilter); @@ -520,11 +520,11 @@ public: int addUserDebugText(char *text, double *textPosition, struct b3RobotSimulatorAddUserDebugTextArgs &args); - int addUserDebugText(char *text, b3Vector3 &textPosition, struct b3RobotSimulatorAddUserDebugTextArgs &args); + int addUserDebugText(char *text, btVector3 &textPosition, struct b3RobotSimulatorAddUserDebugTextArgs &args); int addUserDebugLine(double *fromXYZ, double *toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs &args); - int addUserDebugLine(b3Vector3 &fromXYZ, b3Vector3 &toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs &args); + int addUserDebugLine(btVector3 &fromXYZ, btVector3 &toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs &args); bool setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs &args); @@ -532,11 +532,11 @@ public: bool applyExternalForce(int objectUniqueId, int linkIndex, double *force, double *position, int flags); - bool applyExternalForce(int objectUniqueId, int linkIndex, b3Vector3 &force, b3Vector3 &position, int flags); + bool applyExternalForce(int objectUniqueId, int linkIndex, btVector3 &force, btVector3 &position, int flags); bool applyExternalTorque(int objectUniqueId, int linkIndex, double *torque, int flags); - bool applyExternalTorque(int objectUniqueId, int linkIndex, b3Vector3 &torque, int flags); + bool applyExternalTorque(int objectUniqueId, int linkIndex, btVector3 &torque, int flags); bool enableJointForceTorqueSensor(int bodyUniqueId, int linkIndex, bool enable); @@ -548,11 +548,11 @@ public: bool getOverlappingObjects(double *aabbMin, double *aabbMax, struct b3AABBOverlapData *overlapData); - bool getOverlappingObjects(b3Vector3 &aabbMin, b3Vector3 &aabbMax, struct b3AABBOverlapData *overlapData); + bool getOverlappingObjects(btVector3 &aabbMin, btVector3 &aabbMax, struct b3AABBOverlapData *overlapData); bool getAABB(int bodyUniqueId, int linkIndex, double *aabbMin, double *aabbMax); - bool getAABB(int bodyUniqueId, int linkIndex, b3Vector3 &aabbMin, b3Vector3 &aabbMax); + bool getAABB(int bodyUniqueId, int linkIndex, btVector3 &aabbMin, btVector3 &aabbMax); int createCollisionShape(int shapeType, struct b3RobotSimulatorCreateCollisionShapeArgs &args); diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 222114779..259ad229e 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -140,7 +140,7 @@ public: { m_robotSim.loadURDF("plane.urdf"); } - m_robotSim.setGravity(b3MakeVector3(0,0,-10)); + m_robotSim.setGravity(btVector3(0,0,-10)); m_robotSim.setNumSimulationSubSteps(4); } @@ -181,7 +181,7 @@ public: m_robotSim.loadURDF("plane.urdf"); } - m_robotSim.setGravity(b3MakeVector3(0,0,-10)); + m_robotSim.setGravity(btVector3(0,0,-10)); m_robotSim.setNumSimulationSubSteps(4); } @@ -241,7 +241,7 @@ public: { m_robotSim.loadURDF("plane.urdf"); } - m_robotSim.setGravity(b3MakeVector3(0,0,-10)); + m_robotSim.setGravity(btVector3(0,0,-10)); b3JointInfo revoluteJoint1; revoluteJoint1.m_parentFrame[0] = -0.055; @@ -335,7 +335,7 @@ public: m_robotSim.loadURDF("plane.urdf", args); } - m_robotSim.setGravity(b3MakeVector3(0,0,-10)); + m_robotSim.setGravity(btVector3(0,0,-10)); m_robotSim.loadSoftBody("bunny.obj",0.1,0.1,0.02); b3JointInfo revoluteJoint1; @@ -411,7 +411,7 @@ public: args.m_useMultiBody = false; m_robotSim.loadURDF("plane.urdf", args); } - m_robotSim.setGravity(b3MakeVector3(0,0,-10)); + m_robotSim.setGravity(btVector3(0,0,-10)); m_robotSim.loadSoftBody("bunny.obj",0.3,10.0,0.1); } } diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index 99f24f7de..a0da21621 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -111,7 +111,7 @@ public: { m_robotSim.loadURDF("plane.urdf"); - m_robotSim.setGravity(b3MakeVector3(0,0,0)); + m_robotSim.setGravity(btVector3(0,0,0)); } } diff --git a/examples/RoboticsLearning/R2D2GraspExample.cpp b/examples/RoboticsLearning/R2D2GraspExample.cpp index abe4e6ff5..002dce813 100644 --- a/examples/RoboticsLearning/R2D2GraspExample.cpp +++ b/examples/RoboticsLearning/R2D2GraspExample.cpp @@ -94,7 +94,7 @@ public: m_robotSim.loadURDF("plane.urdf"); } - m_robotSim.setGravity(b3MakeVector3(0,0,-10)); + m_robotSim.setGravity(btVector3(0,0,-10)); } @@ -119,7 +119,7 @@ public: m_robotSim.loadURDF("plane.urdf",args); } - m_robotSim.setGravity(b3MakeVector3(0,0,-10)); + m_robotSim.setGravity(btVector3(0,0,-10)); } if ((m_options & eROBOTIC_LEARN_ROLLING_FRICTION)!=0) @@ -146,7 +146,7 @@ public: m_robotSim.loadURDF("plane.urdf", args); } - m_robotSim.setGravity(b3MakeVector3(0,0,-10)); + m_robotSim.setGravity(btVector3(0,0,-10)); } From dcc9c4d0d9b22b0f0c6b366d47902a86560a1f58 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 27 May 2018 10:55:53 +1000 Subject: [PATCH 2/2] fix HelloBulletRobotics example from b3Vector3 -> btVector3 --- examples/RobotSimulator/HelloBulletRobotics.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/RobotSimulator/HelloBulletRobotics.cpp b/examples/RobotSimulator/HelloBulletRobotics.cpp index 18f886bca..3b131d17f 100644 --- a/examples/RobotSimulator/HelloBulletRobotics.cpp +++ b/examples/RobotSimulator/HelloBulletRobotics.cpp @@ -23,11 +23,11 @@ int main(int argc, char* argv[]) int r2d2Uid = sim->loadURDF("r2d2.urdf"); printf("r2d2 #joints = %d\n", sim->getNumJoints(r2d2Uid)); - b3Vector3 basePosition = b3MakeVector3(0,0,1); - b3Quaternion baseOrientation = b3Quaternion(0,0,0,1); + btVector3 basePosition = btVector3(0,0,1); + btQuaternion baseOrientation = btQuaternion(0,0,0,1); sim->resetBasePositionAndOrientation(r2d2Uid, basePosition, baseOrientation); - sim->setGravity(b3MakeVector3(0,0,-10)); + sim->setGravity(btVector3(0,0,-10)); while (sim->isConnected()) {