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

118
examples/RobotSimulator/b3RobotSimulatorClientAPI.h
View File

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