1 #ifndef B3_ROBOT_SIMULATOR_CLIENT_API_NO_DIRECT_H 2 #define B3_ROBOT_SIMULATOR_CLIENT_API_NO_DIRECT_H 3 4 ///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet 5 ///as documented in the pybullet Quickstart Guide 6 ///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA 7 8 #include "SharedMemoryPublic.h" 9 #include "LinearMath/btVector3.h" 10 #include "LinearMath/btQuaternion.h" 11 #include "LinearMath/btTransform.h" 12 #include "LinearMath/btAlignedObjectArray.h" 13 14 #include <string> 15 16 struct b3RobotSimulatorLoadUrdfFileArgs 17 { 18 btVector3 m_startPosition; 19 btQuaternion m_startOrientation; 20 bool m_forceOverrideFixedBase; 21 bool m_useMultiBody; 22 int m_flags; 23 b3RobotSimulatorLoadUrdfFileArgsb3RobotSimulatorLoadUrdfFileArgs24 b3RobotSimulatorLoadUrdfFileArgs(const btVector3 &startPos, const btQuaternion &startOrn) 25 : m_startPosition(startPos), 26 m_startOrientation(startOrn), 27 m_forceOverrideFixedBase(false), 28 m_useMultiBody(true), 29 m_flags(0) 30 { 31 } 32 b3RobotSimulatorLoadUrdfFileArgsb3RobotSimulatorLoadUrdfFileArgs33 b3RobotSimulatorLoadUrdfFileArgs() 34 : m_startPosition(btVector3(0, 0, 0)), 35 m_startOrientation(btQuaternion(0, 0, 0, 1)), 36 m_forceOverrideFixedBase(false), 37 m_useMultiBody(true), 38 m_flags(0) 39 { 40 } 41 }; 42 43 struct b3RobotSimulatorLoadSdfFileArgs 44 { 45 bool m_forceOverrideFixedBase; 46 bool m_useMultiBody; 47 b3RobotSimulatorLoadSdfFileArgsb3RobotSimulatorLoadSdfFileArgs48 b3RobotSimulatorLoadSdfFileArgs() 49 : m_forceOverrideFixedBase(false), 50 m_useMultiBody(true) 51 { 52 } 53 }; 54 55 struct b3RobotSimulatorLoadSoftBodyArgs 56 { 57 btVector3 m_startPosition; 58 btQuaternion m_startOrientation; 59 double m_scale; 60 double m_mass; 61 double m_collisionMargin; 62 b3RobotSimulatorLoadSoftBodyArgsb3RobotSimulatorLoadSoftBodyArgs63 b3RobotSimulatorLoadSoftBodyArgs(const btVector3 &startPos, const btQuaternion &startOrn, const double &scale, const double &mass, const double &collisionMargin) 64 : m_startPosition(startPos), 65 m_startOrientation(startOrn), 66 m_scale(scale), 67 m_mass(mass), 68 m_collisionMargin(collisionMargin) 69 { 70 } 71 b3RobotSimulatorLoadSoftBodyArgsb3RobotSimulatorLoadSoftBodyArgs72 b3RobotSimulatorLoadSoftBodyArgs(const btVector3 &startPos, const btQuaternion &startOrn) 73 { 74 b3RobotSimulatorLoadSoftBodyArgs(startPos, startOrn, 1.0, 1.0, 0.02); 75 } 76 b3RobotSimulatorLoadSoftBodyArgsb3RobotSimulatorLoadSoftBodyArgs77 b3RobotSimulatorLoadSoftBodyArgs() 78 { 79 b3RobotSimulatorLoadSoftBodyArgs(btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1)); 80 } 81 b3RobotSimulatorLoadSoftBodyArgsb3RobotSimulatorLoadSoftBodyArgs82 b3RobotSimulatorLoadSoftBodyArgs(double scale, double mass, double collisionMargin) 83 : m_startPosition(btVector3(0, 0, 0)), 84 m_startOrientation(btQuaternion(0, 0, 0, 1)), 85 m_scale(scale), 86 m_mass(mass), 87 m_collisionMargin(collisionMargin) 88 { 89 } 90 }; 91 92 93 struct b3RobotSimulatorLoadDeformableBodyArgs 94 { 95 btVector3 m_startPosition; 96 btQuaternion m_startOrientation; 97 double m_scale; 98 double m_mass; 99 double m_collisionMargin; 100 double m_springElasticStiffness; 101 double m_springDampingStiffness; 102 double m_springBendingStiffness; 103 double m_NeoHookeanMu; 104 double m_NeoHookeanLambda; 105 double m_NeoHookeanDamping; 106 bool m_useSelfCollision; 107 bool m_useFaceContact; 108 bool m_useBendingSprings; 109 double m_frictionCoeff; 110 b3RobotSimulatorLoadDeformableBodyArgsb3RobotSimulatorLoadDeformableBodyArgs111 b3RobotSimulatorLoadDeformableBodyArgs(const btVector3 &startPos, const btQuaternion &startOrn, const double &scale, const double &mass, const double &collisionMargin) 112 : m_startPosition(startPos), 113 m_startOrientation(startOrn), 114 m_scale(scale), 115 m_mass(mass), 116 m_collisionMargin(collisionMargin), 117 m_springElasticStiffness(-1), 118 m_springDampingStiffness(-1), 119 m_springBendingStiffness(-1), 120 m_NeoHookeanMu(-1), 121 m_NeoHookeanDamping(-1), 122 m_useSelfCollision(false), 123 m_useFaceContact(false), 124 m_useBendingSprings(false), 125 m_frictionCoeff(0) 126 { 127 } 128 b3RobotSimulatorLoadDeformableBodyArgsb3RobotSimulatorLoadDeformableBodyArgs129 b3RobotSimulatorLoadDeformableBodyArgs(const btVector3 &startPos, const btQuaternion &startOrn) 130 { 131 b3RobotSimulatorLoadSoftBodyArgs(startPos, startOrn, 1.0, 1.0, 0.02); 132 } 133 b3RobotSimulatorLoadDeformableBodyArgsb3RobotSimulatorLoadDeformableBodyArgs134 b3RobotSimulatorLoadDeformableBodyArgs() 135 { 136 b3RobotSimulatorLoadSoftBodyArgs(btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1)); 137 } 138 b3RobotSimulatorLoadDeformableBodyArgsb3RobotSimulatorLoadDeformableBodyArgs139 b3RobotSimulatorLoadDeformableBodyArgs(double scale, double mass, double collisionMargin) 140 : m_startPosition(btVector3(0, 0, 0)), 141 m_startOrientation(btQuaternion(0, 0, 0, 1)), 142 m_scale(scale), 143 m_mass(mass), 144 m_collisionMargin(collisionMargin) 145 { 146 } 147 }; 148 149 150 struct b3RobotSimulatorLoadFileResults 151 { 152 btAlignedObjectArray<int> m_uniqueObjectIds; b3RobotSimulatorLoadFileResultsb3RobotSimulatorLoadFileResults153 b3RobotSimulatorLoadFileResults() 154 { 155 } 156 }; 157 158 struct b3RobotSimulatorChangeVisualShapeArgs 159 { 160 int m_objectUniqueId; 161 int m_linkIndex; 162 int m_shapeIndex; 163 int m_textureUniqueId; 164 btVector4 m_rgbaColor; 165 bool m_hasRgbaColor; 166 btVector3 m_specularColor; 167 bool m_hasSpecularColor; 168 b3RobotSimulatorChangeVisualShapeArgsb3RobotSimulatorChangeVisualShapeArgs169 b3RobotSimulatorChangeVisualShapeArgs() 170 : m_objectUniqueId(-1), 171 m_linkIndex(-1), 172 m_shapeIndex(-1), 173 m_textureUniqueId(-2), 174 m_rgbaColor(0, 0, 0, 1), 175 m_hasRgbaColor(false), 176 m_specularColor(1, 1, 1), 177 m_hasSpecularColor(false) 178 { 179 } 180 }; 181 182 struct b3RobotSimulatorJointMotorArgs 183 { 184 int m_controlMode; 185 186 double m_targetPosition; 187 double m_kp; 188 189 double m_targetVelocity; 190 double m_kd; 191 192 double m_maxTorqueValue; 193 b3RobotSimulatorJointMotorArgsb3RobotSimulatorJointMotorArgs194 b3RobotSimulatorJointMotorArgs(int controlMode) 195 : m_controlMode(controlMode), 196 m_targetPosition(0), 197 m_kp(0.1), 198 m_targetVelocity(0), 199 m_kd(0.9), 200 m_maxTorqueValue(1000) 201 { 202 } 203 }; 204 205 enum b3RobotSimulatorInverseKinematicsFlags 206 { 207 B3_HAS_IK_TARGET_ORIENTATION = 1, 208 B3_HAS_NULL_SPACE_VELOCITY = 2, 209 B3_HAS_JOINT_DAMPING = 4, 210 B3_HAS_CURRENT_POSITIONS = 8, 211 }; 212 213 struct b3RobotSimulatorInverseKinematicArgs 214 { 215 int m_bodyUniqueId; 216 double m_endEffectorTargetPosition[3]; 217 double m_endEffectorTargetOrientation[4]; 218 int m_endEffectorLinkIndex; 219 int m_flags; 220 int m_numDegreeOfFreedom; 221 btAlignedObjectArray<double> m_lowerLimits; 222 btAlignedObjectArray<double> m_upperLimits; 223 btAlignedObjectArray<double> m_jointRanges; 224 btAlignedObjectArray<double> m_restPoses; 225 btAlignedObjectArray<double> m_jointDamping; 226 btAlignedObjectArray<double> m_currentJointPositions; 227 b3RobotSimulatorInverseKinematicArgsb3RobotSimulatorInverseKinematicArgs228 b3RobotSimulatorInverseKinematicArgs() 229 : m_bodyUniqueId(-1), 230 m_endEffectorLinkIndex(-1), 231 m_flags(0) 232 { 233 m_endEffectorTargetPosition[0] = 0; 234 m_endEffectorTargetPosition[1] = 0; 235 m_endEffectorTargetPosition[2] = 0; 236 237 m_endEffectorTargetOrientation[0] = 0; 238 m_endEffectorTargetOrientation[1] = 0; 239 m_endEffectorTargetOrientation[2] = 0; 240 m_endEffectorTargetOrientation[3] = 1; 241 } 242 }; 243 244 struct b3RobotSimulatorInverseKinematicsResults 245 { 246 int m_bodyUniqueId; 247 btAlignedObjectArray<double> m_calculatedJointPositions; 248 }; 249 250 struct b3JointStates2 251 { 252 int m_bodyUniqueId; 253 int m_numDegreeOfFreedomQ; 254 int m_numDegreeOfFreedomU; 255 btTransform m_rootLocalInertialFrame; 256 btAlignedObjectArray<double> m_actualStateQ; 257 btAlignedObjectArray<double> m_actualStateQdot; 258 btAlignedObjectArray<double> m_jointReactionForces; 259 }; 260 261 struct b3RobotSimulatorJointMotorArrayArgs 262 { 263 int m_controlMode; 264 int m_numControlledDofs; 265 266 int *m_jointIndices; 267 268 double *m_targetPositions; 269 double *m_kps; 270 271 double *m_targetVelocities; 272 double *m_kds; 273 274 double *m_forces; 275 b3RobotSimulatorJointMotorArrayArgsb3RobotSimulatorJointMotorArrayArgs276 b3RobotSimulatorJointMotorArrayArgs(int controlMode, int numControlledDofs) 277 : m_controlMode(controlMode), 278 m_numControlledDofs(numControlledDofs), 279 m_jointIndices(NULL), 280 m_targetPositions(NULL), 281 m_kps(NULL), 282 m_targetVelocities(NULL), 283 m_kds(NULL), 284 m_forces(NULL) 285 { 286 } 287 }; 288 289 struct b3RobotSimulatorGetCameraImageArgs 290 { 291 int m_width; 292 int m_height; 293 float *m_viewMatrix; 294 float *m_projectionMatrix; 295 float *m_lightDirection; 296 float *m_lightColor; 297 float m_lightDistance; 298 int m_hasShadow; 299 float m_lightAmbientCoeff; 300 float m_lightDiffuseCoeff; 301 float m_lightSpecularCoeff; 302 int m_renderer; 303 b3RobotSimulatorGetCameraImageArgsb3RobotSimulatorGetCameraImageArgs304 b3RobotSimulatorGetCameraImageArgs(int width, int height) 305 : m_width(width), 306 m_height(height), 307 m_viewMatrix(NULL), 308 m_projectionMatrix(NULL), 309 m_lightDirection(NULL), 310 m_lightColor(NULL), 311 m_lightDistance(-1), 312 m_hasShadow(-1), 313 m_lightAmbientCoeff(-1), 314 m_lightDiffuseCoeff(-1), 315 m_lightSpecularCoeff(-1), 316 m_renderer(-1) 317 { 318 } 319 }; 320 321 struct b3RobotSimulatorSetPhysicsEngineParameters : b3PhysicsSimulationParameters 322 { b3RobotSimulatorSetPhysicsEngineParametersb3RobotSimulatorSetPhysicsEngineParameters323 b3RobotSimulatorSetPhysicsEngineParameters() 324 { 325 m_deltaTime = -1; 326 m_gravityAcceleration[0] = 0; 327 m_gravityAcceleration[1] = 0; 328 m_gravityAcceleration[2] = 0; 329 330 m_numSimulationSubSteps = -1; 331 m_numSolverIterations = -1; 332 m_useRealTimeSimulation = -1; 333 m_useSplitImpulse = -1; 334 m_splitImpulsePenetrationThreshold = -1; 335 m_contactBreakingThreshold = -1; 336 m_internalSimFlags = -1; 337 m_defaultContactERP = -1; 338 m_collisionFilterMode = -1; 339 m_enableFileCaching = -1; 340 m_restitutionVelocityThreshold = -1; 341 m_defaultNonContactERP = -1; 342 m_frictionERP = -1; 343 m_defaultGlobalCFM = -1; 344 m_frictionCFM = -1; 345 m_enableConeFriction = -1; 346 m_deterministicOverlappingPairs = -1; 347 m_allowedCcdPenetration = -1; 348 m_jointFeedbackMode = -1; 349 m_solverResidualThreshold = -1; 350 m_contactSlop = -1; 351 352 m_collisionFilterMode = -1; 353 m_contactBreakingThreshold = -1; 354 355 m_enableFileCaching = -1; 356 m_restitutionVelocityThreshold = -1; 357 358 m_frictionERP = -1; 359 m_solverResidualThreshold = -1; 360 m_constraintSolverType = -1; 361 m_minimumSolverIslandSize = -1; 362 } 363 }; 364 365 struct b3RobotSimulatorChangeDynamicsArgs 366 { 367 double m_mass; 368 double m_lateralFriction; 369 double m_spinningFriction; 370 double m_rollingFriction; 371 double m_restitution; 372 double m_linearDamping; 373 double m_angularDamping; 374 double m_contactStiffness; 375 double m_contactDamping; 376 int m_frictionAnchor; 377 int m_activationState; 378 b3RobotSimulatorChangeDynamicsArgsb3RobotSimulatorChangeDynamicsArgs379 b3RobotSimulatorChangeDynamicsArgs() 380 : m_mass(-1), 381 m_lateralFriction(-1), 382 m_spinningFriction(-1), 383 m_rollingFriction(-1), 384 m_restitution(-1), 385 m_linearDamping(-1), 386 m_angularDamping(-1), 387 m_contactStiffness(-1), 388 m_contactDamping(-1), 389 m_frictionAnchor(-1), 390 m_activationState(-1) 391 { 392 } 393 }; 394 395 struct b3RobotSimulatorAddUserDebugLineArgs 396 { 397 double m_colorRGB[3]; 398 double m_lineWidth; 399 double m_lifeTime; 400 int m_parentObjectUniqueId; 401 int m_parentLinkIndex; 402 b3RobotSimulatorAddUserDebugLineArgsb3RobotSimulatorAddUserDebugLineArgs403 b3RobotSimulatorAddUserDebugLineArgs() 404 : m_lineWidth(1), 405 m_lifeTime(0), 406 m_parentObjectUniqueId(-1), 407 m_parentLinkIndex(-1) 408 { 409 m_colorRGB[0] = 1; 410 m_colorRGB[1] = 1; 411 m_colorRGB[2] = 1; 412 } 413 }; 414 415 enum b3AddUserDebugTextFlags 416 { 417 DEBUG_TEXT_HAS_ORIENTATION = 1 418 }; 419 420 struct b3RobotSimulatorAddUserDebugTextArgs 421 { 422 double m_colorRGB[3]; 423 double m_size; 424 double m_lifeTime; 425 double m_textOrientation[4]; 426 int m_parentObjectUniqueId; 427 int m_parentLinkIndex; 428 int m_flags; 429 b3RobotSimulatorAddUserDebugTextArgsb3RobotSimulatorAddUserDebugTextArgs430 b3RobotSimulatorAddUserDebugTextArgs() 431 : m_size(1), 432 m_lifeTime(0), 433 m_parentObjectUniqueId(-1), 434 m_parentLinkIndex(-1), 435 m_flags(0) 436 { 437 m_colorRGB[0] = 1; 438 m_colorRGB[1] = 1; 439 m_colorRGB[2] = 1; 440 441 m_textOrientation[0] = 0; 442 m_textOrientation[1] = 0; 443 m_textOrientation[2] = 0; 444 m_textOrientation[3] = 1; 445 } 446 }; 447 448 struct b3RobotSimulatorGetContactPointsArgs 449 { 450 int m_bodyUniqueIdA; 451 int m_bodyUniqueIdB; 452 int m_linkIndexA; 453 int m_linkIndexB; 454 b3RobotSimulatorGetContactPointsArgsb3RobotSimulatorGetContactPointsArgs455 b3RobotSimulatorGetContactPointsArgs() 456 : m_bodyUniqueIdA(-1), 457 m_bodyUniqueIdB(-1), 458 m_linkIndexA(-2), 459 m_linkIndexB(-2) 460 { 461 } 462 }; 463 464 struct b3RobotSimulatorCreateCollisionShapeArgs 465 { 466 int m_shapeType; 467 double m_radius; 468 btVector3 m_halfExtents; 469 double m_height; 470 char *m_fileName; 471 btVector3 m_meshScale; 472 btVector3 m_planeNormal; 473 int m_flags; 474 475 double m_heightfieldTextureScaling; 476 btAlignedObjectArray<float> m_heightfieldData; 477 int m_numHeightfieldRows; 478 int m_numHeightfieldColumns; 479 int m_replaceHeightfieldIndex; 480 b3RobotSimulatorCreateCollisionShapeArgsb3RobotSimulatorCreateCollisionShapeArgs481 b3RobotSimulatorCreateCollisionShapeArgs() 482 : m_shapeType(-1), 483 m_radius(0.5), 484 m_height(1), 485 m_fileName(NULL), 486 m_flags(0), 487 m_heightfieldTextureScaling(1), 488 m_numHeightfieldRows(0), 489 m_numHeightfieldColumns(0), 490 m_replaceHeightfieldIndex(-1) 491 { 492 m_halfExtents.m_floats[0] = 1; 493 m_halfExtents.m_floats[1] = 1; 494 m_halfExtents.m_floats[2] = 1; 495 496 m_meshScale.m_floats[0] = 1; 497 m_meshScale.m_floats[1] = 1; 498 m_meshScale.m_floats[2] = 1; 499 500 m_planeNormal.m_floats[0] = 0; 501 m_planeNormal.m_floats[1] = 0; 502 m_planeNormal.m_floats[2] = 1; 503 } 504 }; 505 506 507 struct b3RobotSimulatorCreateVisualShapeArgs 508 { 509 int m_shapeType; 510 double m_radius; 511 btVector3 m_halfExtents; 512 double m_height; 513 char* m_fileName; 514 btVector3 m_meshScale; 515 btVector3 m_planeNormal; 516 int m_flags; b3RobotSimulatorCreateVisualShapeArgsb3RobotSimulatorCreateVisualShapeArgs517 b3RobotSimulatorCreateVisualShapeArgs() 518 : m_shapeType(-1), 519 m_radius(0.5), 520 m_height(1), 521 m_fileName(NULL), 522 m_flags(0) 523 { 524 m_halfExtents.m_floats[0] = 1; 525 m_halfExtents.m_floats[1] = 1; 526 m_halfExtents.m_floats[2] = 1; 527 528 m_meshScale.m_floats[0] = 1; 529 m_meshScale.m_floats[1] = 1; 530 m_meshScale.m_floats[2] = 1; 531 532 m_planeNormal.m_floats[0] = 0; 533 m_planeNormal.m_floats[1] = 0; 534 m_planeNormal.m_floats[2] = 1; 535 } 536 }; 537 538 struct b3RobotSimulatorCreateMultiBodyArgs 539 { 540 double m_baseMass; 541 int m_baseCollisionShapeIndex; 542 int m_baseVisualShapeIndex; 543 btVector3 m_basePosition; 544 btQuaternion m_baseOrientation; 545 btVector3 m_baseInertialFramePosition; 546 btQuaternion m_baseInertialFrameOrientation; 547 548 int m_numLinks; 549 double *m_linkMasses; 550 int *m_linkCollisionShapeIndices; 551 int *m_linkVisualShapeIndices; 552 btVector3 *m_linkPositions; 553 btQuaternion *m_linkOrientations; 554 btVector3 *m_linkInertialFramePositions; 555 btQuaternion *m_linkInertialFrameOrientations; 556 int *m_linkParentIndices; 557 int *m_linkJointTypes; 558 btVector3 *m_linkJointAxes; 559 btAlignedObjectArray<btVector3> m_batchPositions; 560 int m_useMaximalCoordinates; 561 b3RobotSimulatorCreateMultiBodyArgsb3RobotSimulatorCreateMultiBodyArgs562 b3RobotSimulatorCreateMultiBodyArgs() 563 : m_baseMass(0), m_baseCollisionShapeIndex(-1), m_baseVisualShapeIndex(-1), m_numLinks(0), m_linkMasses(NULL), m_linkCollisionShapeIndices(NULL), m_linkVisualShapeIndices(NULL), m_linkPositions(NULL), m_linkOrientations(NULL), m_linkInertialFramePositions(NULL), m_linkInertialFrameOrientations(NULL), m_linkParentIndices(NULL), m_linkJointTypes(NULL), m_linkJointAxes(NULL), m_useMaximalCoordinates(0) 564 { 565 m_basePosition.setValue(0, 0, 0); 566 m_baseOrientation.setValue(0, 0, 0, 1); 567 m_baseInertialFramePosition.setValue(0, 0, 0); 568 m_baseInertialFrameOrientation.setValue(0, 0, 0, 1); 569 } 570 }; 571 572 573 struct b3RobotUserConstraint : public b3UserConstraint 574 { 575 int m_userUpdateFlags;//see EnumUserConstraintFlags 576 setErpb3RobotUserConstraint577 void setErp(double erp) 578 { 579 m_erp = erp; 580 m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_ERP; 581 } 582 setMaxAppliedForceb3RobotUserConstraint583 void setMaxAppliedForce(double maxForce) 584 { 585 m_maxAppliedForce = maxForce; 586 m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_MAX_FORCE; 587 } 588 setGearRatiob3RobotUserConstraint589 void setGearRatio(double gearRatio) 590 { 591 m_gearRatio = gearRatio; 592 m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_GEAR_RATIO; 593 } 594 setGearAuxLinkb3RobotUserConstraint595 void setGearAuxLink(int link) 596 { 597 m_gearAuxLink = link; 598 m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK; 599 } 600 setRelativePositionTargetb3RobotUserConstraint601 void setRelativePositionTarget(double target) 602 { 603 m_relativePositionTarget = target; 604 m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET; 605 } 606 setChildPivotb3RobotUserConstraint607 void setChildPivot(double pivot[3]) 608 { 609 m_childFrame[0] = pivot[0]; 610 m_childFrame[1] = pivot[1]; 611 m_childFrame[2] = pivot[2]; 612 m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_PIVOT_IN_B; 613 } 614 setChildFrameOrientationb3RobotUserConstraint615 void setChildFrameOrientation(double orn[4]) 616 { 617 m_childFrame[3] = orn[0]; 618 m_childFrame[4] = orn[1]; 619 m_childFrame[5] = orn[2]; 620 m_childFrame[6] = orn[3]; 621 m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B; 622 } 623 b3RobotUserConstraintb3RobotUserConstraint624 b3RobotUserConstraint() 625 :m_userUpdateFlags(0) 626 { 627 m_parentBodyIndex = -1; 628 m_parentJointIndex = -1; 629 m_childBodyIndex = -1; 630 m_childJointIndex = -1; 631 //position 632 m_parentFrame[0] = 0; 633 m_parentFrame[1] = 0; 634 m_parentFrame[2] = 0; 635 //orientation quaternion [x,y,z,w] 636 m_parentFrame[3] = 0; 637 m_parentFrame[4] = 0; 638 m_parentFrame[5] = 0; 639 m_parentFrame[6] = 1; 640 641 //position 642 m_childFrame[0] = 0; 643 m_childFrame[1] = 0; 644 m_childFrame[2] = 0; 645 //orientation quaternion [x,y,z,w] 646 m_childFrame[3] = 0; 647 m_childFrame[4] = 0; 648 m_childFrame[5] = 0; 649 m_childFrame[6] = 1; 650 651 m_jointAxis[0] = 0; 652 m_jointAxis[1] = 0; 653 m_jointAxis[2] = 1; 654 655 m_jointType = eFixedType; 656 657 m_maxAppliedForce = 500; 658 m_userConstraintUniqueId = -1; 659 m_gearRatio = -1; 660 m_gearAuxLink = -1; 661 m_relativePositionTarget = 0; 662 m_erp = 0; 663 } 664 }; 665 666 struct b3RobotJointInfo : public b3JointInfo 667 { b3RobotJointInfob3RobotJointInfo668 b3RobotJointInfo() 669 { 670 m_linkName[0] = 0; 671 m_jointName[0] = 0; 672 m_jointType = eFixedType; 673 m_qIndex = -1; 674 m_uIndex = -1; 675 m_jointIndex = -1; 676 m_flags = 0; 677 m_jointDamping = 0; 678 m_jointFriction = 0; 679 m_jointLowerLimit = 1; 680 m_jointUpperLimit = -1; 681 m_jointMaxForce = 500; 682 m_jointMaxVelocity = 100; 683 m_parentIndex = -1; 684 685 //position 686 m_parentFrame[0] = 0; 687 m_parentFrame[1] = 0; 688 m_parentFrame[2] = 0; 689 //orientation quaternion [x,y,z,w] 690 m_parentFrame[3] = 0; 691 m_parentFrame[4] = 0; 692 m_parentFrame[5] = 0; 693 m_parentFrame[6] = 1; 694 695 //position 696 m_childFrame[0] = 0; 697 m_childFrame[1] = 0; 698 m_childFrame[2] = 0; 699 //orientation quaternion [x,y,z,w] 700 m_childFrame[3] = 0; 701 m_childFrame[4] = 0; 702 m_childFrame[5] = 0; 703 m_childFrame[6] = 1; 704 705 m_jointAxis[0] = 0; 706 m_jointAxis[1] = 0; 707 m_jointAxis[2] = 1; 708 } 709 }; 710 711 class b3RobotSimulatorClientAPI_NoDirect 712 { 713 protected: 714 struct b3RobotSimulatorClientAPI_InternalData *m_data; 715 716 public: 717 b3RobotSimulatorClientAPI_NoDirect(); 718 virtual ~b3RobotSimulatorClientAPI_NoDirect(); 719 720 //No 'connect', use setInternalData to bypass the connect method, pass an existing client 721 virtual void setInternalData(struct b3RobotSimulatorClientAPI_InternalData *data); 722 723 void disconnect(); 724 725 bool isConnected() const; 726 727 void setTimeOut(double timeOutInSec); 728 729 void syncBodies(); 730 731 void resetSimulation(); 732 733 void resetSimulation(int flag); 734 735 btQuaternion getQuaternionFromEuler(const btVector3 &rollPitchYaw); 736 btVector3 getEulerFromQuaternion(const btQuaternion &quat); 737 738 int loadURDF(const std::string &fileName, const struct b3RobotSimulatorLoadUrdfFileArgs &args = b3RobotSimulatorLoadUrdfFileArgs()); 739 bool loadSDF(const std::string &fileName, b3RobotSimulatorLoadFileResults &results, const struct b3RobotSimulatorLoadSdfFileArgs &args = b3RobotSimulatorLoadSdfFileArgs()); 740 bool loadMJCF(const std::string &fileName, b3RobotSimulatorLoadFileResults &results); 741 bool loadBullet(const std::string &fileName, b3RobotSimulatorLoadFileResults &results); 742 bool saveBullet(const std::string &fileName); 743 744 int loadTexture(const std::string &fileName); 745 746 bool changeVisualShape(const struct b3RobotSimulatorChangeVisualShapeArgs &args); 747 748 bool savePythonWorld(const std::string &fileName); 749 750 bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo *bodyInfo); 751 752 bool getBasePositionAndOrientation(int bodyUniqueId, btVector3 &basePosition, btQuaternion &baseOrientation) const; 753 bool resetBasePositionAndOrientation(int bodyUniqueId, const btVector3 &basePosition, const btQuaternion &baseOrientation); 754 755 bool getBaseVelocity(int bodyUniqueId, btVector3 &baseLinearVelocity, btVector3 &baseAngularVelocity) const; 756 bool resetBaseVelocity(int bodyUniqueId, const btVector3 &linearVelocity, const btVector3 &angularVelocity) const; 757 758 int getNumJoints(int bodyUniqueId) const; 759 760 bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo *jointInfo); 761 762 int createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo *jointInfo); 763 764 int changeConstraint(int constraintId, b3RobotUserConstraint*jointInfo); 765 766 void removeConstraint(int constraintId); 767 768 bool getConstraintInfo(int constraintUniqueId, struct b3UserConstraint &constraintInfo); 769 770 bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState *state); 771 772 bool getJointStates(int bodyUniqueId, b3JointStates2 &state); 773 774 bool resetJointState(int bodyUniqueId, int jointIndex, double targetValue); 775 776 void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs &args); 777 778 bool setJointMotorControlArray(int bodyUniqueId, int controlMode, int numControlledDofs, 779 int *jointIndices, double *targetVelocities, double *targetPositions, 780 double *forces, double *kps, double *kds); 781 782 void stepSimulation(); 783 784 bool canSubmitCommand() const; 785 786 void setRealTimeSimulation(bool enableRealTimeSimulation); 787 788 void setInternalSimFlags(int flags); 789 790 void setGravity(const btVector3 &gravityAcceleration); 791 792 void setTimeStep(double timeStepInSeconds); 793 void setNumSimulationSubSteps(int numSubSteps); 794 void setNumSolverIterations(int numIterations); 795 void setContactBreakingThreshold(double threshold); 796 797 int computeDofCount(int bodyUniqueId) const; 798 799 bool calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs &args, struct b3RobotSimulatorInverseKinematicsResults &results); 800 801 int calculateMassMatrix(int bodyUniqueId, const double* jointPositions, int numJointPositions, double* massMatrix, int flags); 802 803 bool getBodyJacobian(int bodyUniqueId, int linkIndex, const double *localPosition, const double *jointPositions, const double *jointVelocities, const double *jointAccelerations, double *linearJacobian, double *angularJacobian); 804 805 void configureDebugVisualizer(enum b3ConfigureDebugVisualizerEnum flag, int enable); 806 void resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const btVector3 &targetPos); 807 808 int startStateLogging(b3StateLoggingType loggingType, const std::string &fileName, const btAlignedObjectArray<int> &objectUniqueIds = btAlignedObjectArray<int>(), int maxLogDof = -1); 809 void stopStateLogging(int stateLoggerUniqueId); 810 811 void getVREvents(b3VREventsData *vrEventsData, int deviceTypeFilter); 812 void getKeyboardEvents(b3KeyboardEventsData *keyboardEventsData); 813 814 void submitProfileTiming(const std::string &profileName); 815 816 // JFC: added these 24 methods 817 818 void getMouseEvents(b3MouseEventsData *mouseEventsData); 819 820 bool getLinkState(int bodyUniqueId, int linkIndex, int computeLinkVelocity, int computeForwardKinematics, b3LinkState *linkState); 821 822 bool getCameraImage(int width, int height, struct b3RobotSimulatorGetCameraImageArgs args, b3CameraImageData &imageData); 823 824 bool calculateInverseDynamics(int bodyUniqueId, double *jointPositions, double *jointVelocities, double *jointAccelerations, double *jointForcesOutput); 825 826 int getNumBodies() const; 827 828 int getBodyUniqueId(int bodyId) const; 829 830 bool removeBody(int bodyUniqueId); 831 832 bool getDynamicsInfo(int bodyUniqueId, int linkIndex, b3DynamicsInfo *dynamicsInfo); 833 834 bool changeDynamics(int bodyUniqueId, int linkIndex, struct b3RobotSimulatorChangeDynamicsArgs &args); 835 836 int addUserDebugParameter(const char *paramName, double rangeMin, double rangeMax, double startValue); 837 838 double readUserDebugParameter(int itemUniqueId); 839 840 bool removeUserDebugItem(int itemUniqueId); 841 842 int addUserDebugText(const char *text, double *textPosition, struct b3RobotSimulatorAddUserDebugTextArgs &args); 843 844 int addUserDebugText(const char *text, btVector3 &textPosition, struct b3RobotSimulatorAddUserDebugTextArgs &args); 845 846 int addUserDebugLine(double *fromXYZ, double *toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs &args); 847 848 int addUserDebugLine(btVector3 &fromXYZ, btVector3 &toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs &args); 849 850 bool setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs &args); 851 852 bool setPhysicsEngineParameter(const struct b3RobotSimulatorSetPhysicsEngineParameters &args); 853 854 bool getPhysicsEngineParameters(struct b3RobotSimulatorSetPhysicsEngineParameters &args); 855 856 bool applyExternalForce(int objectUniqueId, int linkIndex, double *force, double *position, int flags); 857 858 bool applyExternalForce(int objectUniqueId, int linkIndex, btVector3 &force, btVector3 &position, int flags); 859 860 bool applyExternalTorque(int objectUniqueId, int linkIndex, double *torque, int flags); 861 862 bool applyExternalTorque(int objectUniqueId, int linkIndex, btVector3 &torque, int flags); 863 864 bool enableJointForceTorqueSensor(int bodyUniqueId, int jointIndex, bool enable); 865 866 bool getDebugVisualizerCamera(struct b3OpenGLVisualizerCameraInfo *cameraInfo); 867 868 bool getContactPoints(struct b3RobotSimulatorGetContactPointsArgs &args, struct b3ContactInformation *contactInfo); 869 870 bool getClosestPoints(struct b3RobotSimulatorGetContactPointsArgs &args, double distance, struct b3ContactInformation *contactInfo); 871 872 bool getOverlappingObjects(double *aabbMin, double *aabbMax, struct b3AABBOverlapData *overlapData); 873 874 bool getOverlappingObjects(btVector3 &aabbMin, btVector3 &aabbMax, struct b3AABBOverlapData *overlapData); 875 876 bool getAABB(int bodyUniqueId, int linkIndex, double *aabbMin, double *aabbMax); 877 878 bool getAABB(int bodyUniqueId, int linkIndex, btVector3 &aabbMin, btVector3 &aabbMax); 879 880 int createVisualShape(int shapeType, struct b3RobotSimulatorCreateVisualShapeArgs& args); 881 882 int createCollisionShape(int shapeType, struct b3RobotSimulatorCreateCollisionShapeArgs &args); 883 884 int createMultiBody(struct b3RobotSimulatorCreateMultiBodyArgs &args); 885 886 int getNumConstraints() const; 887 888 int getConstraintUniqueId(int serialIndex); 889 890 void loadSoftBody(const std::string &fileName, const struct b3RobotSimulatorLoadSoftBodyArgs &args); 891 892 void loadDeformableBody(const std::string &fileName, const struct b3RobotSimulatorLoadDeformableBodyArgs &args); 893 894 virtual void setGuiHelper(struct GUIHelperInterface *guiHelper); 895 virtual struct GUIHelperInterface *getGuiHelper(); 896 897 bool getCollisionShapeData(int bodyUniqueId, int linkIndex, b3CollisionShapeInformation &collisionShapeInfo); 898 899 bool getVisualShapeData(int bodyUniqueId, struct b3VisualShapeInformation &visualShapeInfo); 900 901 int saveStateToMemory(); 902 void restoreStateFromMemory(int stateId); 903 void removeState(int stateUniqueId); 904 getAPIVersion()905 int getAPIVersion() const 906 { 907 return SHARED_MEMORY_MAGIC_NUMBER; 908 } 909 void setAdditionalSearchPath(const std::string &path); 910 911 void setCollisionFilterGroupMask(int bodyUniqueIdA, int linkIndexA, int collisionFilterGroup, int collisionFilterMask); 912 }; 913 914 #endif //B3_ROBOT_SIMULATOR_CLIENT_API_NO_DIRECT_H 915