diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 958cf38a4..223a0a5d5 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -789,22 +789,6 @@ struct CalculateInverseKinematicsResultArgs double m_jointPositions[MAX_DEGREE_OF_FREEDOM]; }; -enum EnumUserConstraintFlags -{ - USER_CONSTRAINT_ADD_CONSTRAINT = 1, - USER_CONSTRAINT_REMOVE_CONSTRAINT = 2, - USER_CONSTRAINT_CHANGE_CONSTRAINT = 4, - USER_CONSTRAINT_CHANGE_PIVOT_IN_B = 8, - USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B = 16, - USER_CONSTRAINT_CHANGE_MAX_FORCE = 32, - USER_CONSTRAINT_REQUEST_INFO = 64, - USER_CONSTRAINT_CHANGE_GEAR_RATIO = 128, - USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK = 256, - USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET = 512, - USER_CONSTRAINT_CHANGE_ERP = 1024, - USER_CONSTRAINT_REQUEST_STATE = 2048, - USER_CONSTRAINT_ADD_SOFT_BODY_ANCHOR = 4096, -}; enum EnumBodyChangeFlags { diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 01727fe5c..63a7cfa55 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -309,6 +309,23 @@ struct b3UserDataValue const char* m_data1; }; +enum EnumUserConstraintFlags +{ + USER_CONSTRAINT_ADD_CONSTRAINT = 1, + USER_CONSTRAINT_REMOVE_CONSTRAINT = 2, + USER_CONSTRAINT_CHANGE_CONSTRAINT = 4, + USER_CONSTRAINT_CHANGE_PIVOT_IN_B = 8, + USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B = 16, + USER_CONSTRAINT_CHANGE_MAX_FORCE = 32, + USER_CONSTRAINT_REQUEST_INFO = 64, + USER_CONSTRAINT_CHANGE_GEAR_RATIO = 128, + USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK = 256, + USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET = 512, + USER_CONSTRAINT_CHANGE_ERP = 1024, + USER_CONSTRAINT_REQUEST_STATE = 2048, + USER_CONSTRAINT_ADD_SOFT_BODY_ANCHOR = 4096, +}; + struct b3UserConstraint { int m_parentBodyIndex; diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp index 9f04cf6ce..4e81e599a 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp @@ -620,7 +620,7 @@ int b3RobotSimulatorClientAPI_NoDirect::createConstraint(int parentBodyIndex, in return -1; } -int b3RobotSimulatorClientAPI_NoDirect::changeConstraint(int constraintId, b3JointInfo* jointInfo) +int b3RobotSimulatorClientAPI_NoDirect::changeConstraint(int constraintId, b3RobotUserConstraint* jointInfo) { if (!isConnected()) { @@ -629,16 +629,35 @@ int b3RobotSimulatorClientAPI_NoDirect::changeConstraint(int constraintId, b3Joi } b3SharedMemoryCommandHandle commandHandle = b3InitChangeUserConstraintCommand(m_data->m_physicsClientHandle, constraintId); - if (jointInfo->m_flags & eJointChangeMaxForce) + if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) { - b3InitChangeUserConstraintSetMaxForce(commandHandle, jointInfo->m_jointMaxForce); + b3InitChangeUserConstraintSetMaxForce(commandHandle, jointInfo->m_maxAppliedForce); } - if (jointInfo->m_flags & eJointChangeChildFramePosition) + if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO) + { + b3InitChangeUserConstraintSetGearRatio(commandHandle, jointInfo->m_gearRatio); + } + + if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_ERP) + { + b3InitChangeUserConstraintSetERP(commandHandle, jointInfo->m_erp); + } + if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK) + { + b3InitChangeUserConstraintSetGearAuxLink(commandHandle, jointInfo->m_gearAuxLink); + } + + if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET) + { + b3InitChangeUserConstraintSetRelativePositionTarget(commandHandle, jointInfo->m_relativePositionTarget); + } + + if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B) { b3InitChangeUserConstraintSetPivotInB(commandHandle, &jointInfo->m_childFrame[0]); } - if (jointInfo->m_flags & eJointChangeChildFrameOrientation) + if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B) { b3InitChangeUserConstraintSetFrameInB(commandHandle, &jointInfo->m_childFrame[3]); } diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h index c23a7af0d..78f729735 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h @@ -526,6 +526,100 @@ struct b3RobotSimulatorCreateMultiBodyArgs } }; + +struct b3RobotUserConstraint : public b3UserConstraint +{ + int m_userUpdateFlags;//see EnumUserConstraintFlags + + void setErp(double erp) + { + m_erp = erp; + m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_ERP; + } + + void setMaxAppliedForce(double maxForce) + { + m_maxAppliedForce = maxForce; + m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_MAX_FORCE; + } + + void setGearRatio(double gearRatio) + { + m_gearRatio = gearRatio; + m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_GEAR_RATIO; + } + + void setGearAuxLink(int link) + { + m_gearAuxLink = link; + m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK; + } + + void setRelativePositionTarget(double target) + { + m_relativePositionTarget = target; + m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET; + } + + void setChildPivot(double pivot[3]) + { + m_childFrame[0] = pivot[0]; + m_childFrame[1] = pivot[1]; + m_childFrame[2] = pivot[2]; + m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_PIVOT_IN_B; + } + + void setChildFrameOrientation(double orn[4]) + { + m_childFrame[3] = orn[0]; + m_childFrame[4] = orn[1]; + m_childFrame[5] = orn[2]; + m_childFrame[6] = orn[3]; + m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B; + } + + b3RobotUserConstraint() + :m_userUpdateFlags(0) + { + m_parentBodyIndex = -1; + m_parentJointIndex = -1; + m_childBodyIndex = -1; + m_childJointIndex = -1; + //position + m_parentFrame[0] = 0; + m_parentFrame[1] = 0; + m_parentFrame[2] = 0; + //orientation quaternion [x,y,z,w] + m_parentFrame[3] = 0; + m_parentFrame[4] = 0; + m_parentFrame[5] = 0; + m_parentFrame[6] = 1; + + //position + m_childFrame[0] = 0; + m_childFrame[1] = 0; + m_childFrame[2] = 0; + //orientation quaternion [x,y,z,w] + m_childFrame[3] = 0; + m_childFrame[4] = 0; + m_childFrame[5] = 0; + m_childFrame[6] = 1; + + m_jointAxis[0] = 0; + m_jointAxis[1] = 0; + m_jointAxis[2] = 1; + + m_jointType = eFixedType; + + m_maxAppliedForce = 500; + m_userConstraintUniqueId = -1; + m_gearRatio = -1; + m_gearAuxLink = -1; + m_relativePositionTarget = 0; + m_erp = 0; + } +}; + struct b3RobotJointInfo : public b3JointInfo { b3RobotJointInfo() @@ -624,7 +718,7 @@ public: int createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo *jointInfo); - int changeConstraint(int constraintId, b3JointInfo *jointInfo); + int changeConstraint(int constraintId, b3RobotUserConstraint*jointInfo); void removeConstraint(int constraintId);