From 8a265b8af2df64163c0eed838a4de0a694fa614c Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 26 Sep 2017 19:54:36 -0700 Subject: [PATCH] expose gear erp/relative position target to C-API/pybullet finish much better C++ vrSyncPlugin, running in-the-loop with the physics at high frequency, see also vr_kuka_setup_vrSyncPlugin.py --- data/pr2_gripper.urdf | 4 +- examples/SharedMemory/PhysicsClientC_API.cpp | 25 ++++ examples/SharedMemory/PhysicsClientC_API.h | 2 + .../PhysicsServerCommandProcessor.cpp | 9 ++ examples/SharedMemory/SharedMemoryCommands.h | 4 +- examples/SharedMemory/SharedMemoryPublic.h | 2 + .../plugins/vrSyncPlugin/vrSyncPlugin.cpp | 123 +++++++++++++++--- .../examples/vr_kuka_setup_vrSyncPlugin.py | 107 +++++++++++++++ examples/pybullet/pybullet.c | 16 ++- .../Featherstone/btMultiBodyConstraint.h | 4 +- .../btMultiBodyGearConstraint.cpp | 21 ++- .../Featherstone/btMultiBodyGearConstraint.h | 15 ++- 12 files changed, 301 insertions(+), 31 deletions(-) create mode 100644 examples/pybullet/examples/vr_kuka_setup_vrSyncPlugin.py diff --git a/data/pr2_gripper.urdf b/data/pr2_gripper.urdf index 7d16d986a..4913718ce 100644 --- a/data/pr2_gripper.urdf +++ b/data/pr2_gripper.urdf @@ -24,7 +24,7 @@ - + @@ -83,7 +83,7 @@ - + diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 1270a8021..d8c2fff7f 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2131,6 +2131,31 @@ B3_SHARED_API int b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommand return 0; } +B3_SHARED_API int b3InitChangeUserConstraintSetRelativePositionTarget(b3SharedMemoryCommandHandle commandHandle, double relativePositionTarget) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_USER_CONSTRAINT); + b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT); + command->m_updateFlags |=USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET; + command->m_userConstraintArguments.m_relativePositionTarget = relativePositionTarget; + + return 0; +} +B3_SHARED_API int b3InitChangeUserConstraintSetERP(b3SharedMemoryCommandHandle commandHandle, double erp) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_USER_CONSTRAINT); + b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT); + + command->m_updateFlags |=USER_CONSTRAINT_CHANGE_ERP; + command->m_userConstraintArguments.m_erp = erp; + + return 0; +} + + B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index d2b2c8421..d18a547fe 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -136,6 +136,8 @@ B3_SHARED_API int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHan B3_SHARED_API int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce); B3_SHARED_API int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio); B3_SHARED_API int b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommandHandle commandHandle, int gearAuxLink); +B3_SHARED_API int b3InitChangeUserConstraintSetRelativePositionTarget(b3SharedMemoryCommandHandle commandHandle, double relativePositionTarget); +B3_SHARED_API int b3InitChangeUserConstraintSetERP(b3SharedMemoryCommandHandle commandHandle, double erp); B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 012169251..7f3223b59 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -7278,6 +7278,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { userConstraintPtr->m_mbConstraint->setGearRatio(clientCmd.m_userConstraintArguments.m_gearRatio); } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET) + { + userConstraintPtr->m_mbConstraint->setRelativePositionTarget(clientCmd.m_userConstraintArguments.m_relativePositionTarget); + } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_ERP) + { + userConstraintPtr->m_mbConstraint->setErp(clientCmd.m_userConstraintArguments.m_erp); + } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK) { userConstraintPtr->m_mbConstraint->setGearAuxLink(clientCmd.m_userConstraintArguments.m_gearAuxLink); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 49098e0a0..b196d6c71 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -678,7 +678,9 @@ enum EnumUserConstraintFlags 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_GEAR_AUX_LINK=256, + USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET=512, + USER_CONSTRAINT_CHANGE_ERP=1024, }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 90dbfe8b9..28b7ec5b1 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -247,6 +247,8 @@ struct b3UserConstraint int m_userConstraintUniqueId; double m_gearRatio; int m_gearAuxLink; + double m_relativePositionTarget; + double m_erp; }; diff --git a/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp b/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp index ec14a5165..474e944df 100644 --- a/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp +++ b/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp @@ -6,7 +6,6 @@ //plugin = p.loadPlugin("e:/develop/bullet3/bin/pybullet_vrSyncPlugin_vs2010_x64_release.dll") //could also be plugin = p.loadPlugin("vrSyncPlugin.so") on Mac/Linux //controllerId = 3 -//p.executePluginCommand(plugin ,"bla", [controllerId,pr2_cid],[50]) #include "vrSyncPlugin.h" #include "../../SharedMemoryPublic.h" @@ -19,12 +18,18 @@ struct MyClass int m_controllerId; int m_constraintId; + int m_constraintId2; + int m_gripperId; float m_maxForce; + float m_maxForce2; MyClass() :m_testData(42), m_controllerId(-1), m_constraintId(-1), - m_maxForce(0) + m_constraintId2(-1), + m_gripperId(-1), + m_maxForce(0), + m_maxForce2(0) { } virtual ~MyClass() @@ -56,7 +61,7 @@ B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context) if (statusType == CMD_REQUEST_VR_EVENTS_DATA_COMPLETED) { struct b3VREventsData vrEvents; - + int i = 0; b3GetVREventsData(context->m_physClient, &vrEvents); if (vrEvents.m_numControllerEvents) @@ -66,18 +71,95 @@ B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context) b3VRControllerEvent& event = vrEvents.m_controllerEvents[n]; if (event.m_controllerId ==obj->m_controllerId) { - b3SharedMemoryCommandHandle commandHandle; - int userConstraintUniqueId = obj->m_constraintId; - commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, userConstraintUniqueId); - double pos[4] = {event.m_pos[0],event.m_pos[1],event.m_pos[2],1}; - b3InitChangeUserConstraintSetPivotInB(commandHandle, pos); - double orn[4] = {event.m_orn[0],event.m_orn[1],event.m_orn[2],event.m_orn[3]}; - b3InitChangeUserConstraintSetFrameInB(commandHandle, orn); - b3InitChangeUserConstraintSetMaxForce(commandHandle, obj->m_maxForce); + if (obj->m_constraintId>=0) + { + //this is basically equivalent to doing this in Python/pybullet: + //p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=...) + b3SharedMemoryCommandHandle commandHandle; + int userConstraintUniqueId = obj->m_constraintId; + commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, userConstraintUniqueId); + double pos[4] = {event.m_pos[0],event.m_pos[1],event.m_pos[2],1}; + b3InitChangeUserConstraintSetPivotInB(commandHandle, pos); + double orn[4] = {event.m_orn[0],event.m_orn[1],event.m_orn[2],event.m_orn[3]}; + b3InitChangeUserConstraintSetFrameInB(commandHandle, orn); + b3InitChangeUserConstraintSetMaxForce(commandHandle, obj->m_maxForce); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle); + } + // apply the analogue button to close the constraint, using a gear constraint with position target + if (obj->m_constraintId2>=0) + { + //this block is similar to + //p.changeConstraint(c,gearRatio=1, erp=..., relativePositionTarget=relPosTarget, maxForce=...) + //printf("obj->m_constraintId2=%d\n", obj->m_constraintId2); + b3SharedMemoryCommandHandle commandHandle; + commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, obj->m_constraintId2); + + //0 -> open, 1 = closed + double openPos = 1.; + double relPosTarget = openPos - (event.m_analogAxis*openPos); + b3InitChangeUserConstraintSetRelativePositionTarget(commandHandle, relPosTarget); + b3InitChangeUserConstraintSetERP(commandHandle,1); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle); + } + //printf("event.m_analogAxis=%f\n", event.m_analogAxis); + + // use the pr2_gripper motors to keep the gripper centered/symmetric around the center axis + if (obj->m_gripperId>=0) + { + //this block is similar to + //b = p.getJointState(pr2_gripper,2)[0] + //print("b = " + str(b)) + //p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3) + + //printf("obj->m_gripperId=%d\n", obj->m_gripperId); + { + b3SharedMemoryCommandHandle cmd_handle = + b3RequestActualStateCommandInit(context->m_physClient, obj->m_gripperId); + b3SharedMemoryStatusHandle status_handle = + b3SubmitClientCommandAndWaitStatus(context->m_physClient, cmd_handle); + + int status_type = b3GetStatusType(status_handle); + if (status_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + //printf("status_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED\n"); + + b3JointSensorState sensorState; + if (b3GetJointState(context->m_physClient, status_handle, 2, &sensorState)) + { + + + b3SharedMemoryCommandHandle commandHandle; + double targetPosition = sensorState.m_jointPosition; + //printf("targetPosition =%f\n", targetPosition); + if (1) + { + b3JointInfo info; + b3GetJointInfo(context->m_physClient, obj->m_gripperId, 0, &info); + commandHandle = b3JointControlCommandInit2(context->m_physClient, obj->m_gripperId, CONTROL_MODE_POSITION_VELOCITY_PD); + double kp = .1; + double targetVelocity = 0; + double kd = .6; + b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition); + b3JointControlSetKp(commandHandle, info.m_uIndex, kp); + b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex,targetVelocity); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, obj->m_maxForce2); + b3SubmitClientCommandAndWaitStatus(context->m_physClient, cmd_handle); + } + } else + { + //printf("???\n"); + } + + } else + { + //printf("no\n"); + } + + } + + } - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle); - //this is basically equivalent to doing this in Python/pybullet: - //p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=500) } } } @@ -92,15 +174,26 @@ B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context) B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const struct b3PluginArguments* arguments) { MyClass* obj = (MyClass*) context->m_userPointer; - if (arguments->m_numInts>=2 && arguments->m_numFloats >= 0) + if (arguments->m_numInts>=4 && arguments->m_numFloats >= 2) { obj->m_constraintId = arguments->m_ints[1]; + obj->m_constraintId2 = arguments->m_ints[2]; + obj->m_gripperId = arguments->m_ints[3]; printf("obj->m_constraintId=%d\n", obj->m_constraintId); obj->m_maxForce = arguments->m_floats[0]; + obj->m_maxForce2 = arguments->m_floats[1]; printf("obj->m_maxForce = %f\n", obj->m_maxForce); obj->m_controllerId = arguments->m_ints[0]; printf("obj->m_controllerId=%d\n", obj->m_controllerId); + b3SharedMemoryCommandHandle command = b3InitSyncBodyInfoCommand(context->m_physClient); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, command); + int statusType = b3GetStatusType(statusHandle); + + if (statusType != CMD_SYNC_BODY_INFO_COMPLETED) + { + + } } return 0; } diff --git a/examples/pybullet/examples/vr_kuka_setup_vrSyncPlugin.py b/examples/pybullet/examples/vr_kuka_setup_vrSyncPlugin.py new file mode 100644 index 000000000..7297bdd73 --- /dev/null +++ b/examples/pybullet/examples/vr_kuka_setup_vrSyncPlugin.py @@ -0,0 +1,107 @@ +import pybullet as p +import time +#p.connect(p.UDP,"192.168.86.100") + + +cid = p.connect(p.SHARED_MEMORY) +if (cid<0): + p.connect(p.GUI) +p.resetSimulation() +#disable rendering during loading makes it much faster +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) +objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)] +#objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)] +objects = [p.loadURDF("pr2_gripper.urdf", 0.500000,0.300006,0.700000,-0.000000,-0.000000,-0.000031,1.000000)] +pr2_gripper = objects[0] +print ("pr2_gripper=") +print (pr2_gripper) + +jointPositions=[ 0.550569, 0.000000, 0.549657, 0.000000 ] +for jointIndex in range (p.getNumJoints(pr2_gripper)): + p.resetJointState(pr2_gripper,jointIndex,jointPositions[jointIndex]) + p.setJointMotorControl2(pr2_gripper,jointIndex,p.POSITION_CONTROL,targetPosition=0,force=0) + +pr2_cid = p.createConstraint(pr2_gripper,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.2,0,0],[0.500000,0.300006,0.700000]) +print ("pr2_cid") +print (pr2_cid) + +pr2_cid2 = p.createConstraint(pr2_gripper,0,pr2_gripper,2,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(pr2_cid2,gearRatio=1, erp=0.5, relativePositionTarget=0.5, maxForce=3) + + + +objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)] +kuka = objects[0] +jointPositions=[ -0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001 ] +for jointIndex in range (p.getNumJoints(kuka)): + p.resetJointState(kuka,jointIndex,jointPositions[jointIndex]) + p.setJointMotorControl2(kuka,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0) + +objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.700000,0.000000,0.000000,0.000000,1.000000)] +objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.800000,0.000000,0.000000,0.000000,1.000000)] +objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.900000,0.000000,0.000000,0.000000,1.000000)] +objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf") +kuka_gripper = objects[0] +print ("kuka gripper=") +print(kuka_gripper) + +p.resetBasePositionAndOrientation(kuka_gripper,[0.923103,-0.200000,1.250036],[-0.000000,0.964531,-0.000002,-0.263970]) +jointPositions=[ 0.000000, -0.011130, -0.206421, 0.205143, -0.009999, 0.000000, -0.010055, 0.000000 ] +for jointIndex in range (p.getNumJoints(kuka_gripper)): + p.resetJointState(kuka_gripper,jointIndex,jointPositions[jointIndex]) + p.setJointMotorControl2(kuka_gripper,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0) + + +kuka_cid = p.createConstraint(kuka, 6, kuka_gripper,0,p.JOINT_FIXED, [0,0,0], [0,0,0.05],[0,0,0]) + +objects = [p.loadURDF("jenga/jenga.urdf", 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] +objects = [p.loadURDF("jenga/jenga.urdf", 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] +objects = [p.loadURDF("jenga/jenga.urdf", 1.100000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] +objects = [p.loadURDF("jenga/jenga.urdf", 1.000000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] +objects = [p.loadURDF("jenga/jenga.urdf", 0.900000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] +objects = [p.loadURDF("jenga/jenga.urdf", 0.800000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] +objects = [p.loadURDF("table/table.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)] +objects = [p.loadURDF("teddy_vhacd.urdf", 1.050000,-0.500000,0.700000,0.000000,0.000000,0.707107,0.707107)] +objects = [p.loadURDF("cube_small.urdf", 0.950000,-0.100000,0.700000,0.000000,0.000000,0.707107,0.707107)] +objects = [p.loadURDF("sphere_small.urdf", 0.850000,-0.400000,0.700000,0.000000,0.000000,0.707107,0.707107)] +objects = [p.loadURDF("duck_vhacd.urdf", 0.850000,-0.400000,0.900000,0.000000,0.000000,0.707107,0.707107)] +objects = p.loadSDF("kiva_shelf/model.sdf") +ob = objects[0] +p.resetBasePositionAndOrientation(ob,[0.000000,1.000000,1.204500],[0.000000,0.000000,0.000000,1.000000]) +objects = [p.loadURDF("teddy_vhacd.urdf", -0.100000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)] +objects = [p.loadURDF("sphere_small.urdf", -0.100000,0.955006,1.169706,0.633232,-0.000000,-0.000000,0.773962)] +objects = [p.loadURDF("cube_small.urdf", 0.300000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)] +objects = [p.loadURDF("table_square/table_square.urdf", -1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)] +ob = objects[0] +jointPositions=[ 0.000000 ] +for jointIndex in range (p.getNumJoints(ob)): + p.resetJointState(ob,jointIndex,jointPositions[jointIndex]) + +objects = [p.loadURDF("husky/husky.urdf", 2.000000,-5.000000,1.000000,0.000000,0.000000,0.000000,1.000000)] +ob = objects[0] +jointPositions=[ 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000 ] +for jointIndex in range (p.getNumJoints(ob)): + p.resetJointState(ob,jointIndex,jointPositions[jointIndex]) + +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) + +p.setGravity(0.000000,0.000000,0.000000) +p.setGravity(0,0,-10) + +##show this for 10 seconds +#now = time.time() +#while (time.time() < now+10): +# p.stepSimulation() +p.setRealTimeSimulation(1) + + +plugin = p.loadPlugin("e:/develop/bullet3/bin/pybullet_vrSyncPlugin_vs2010_x64_release.dll") +controllerId = 3 +p.executePluginCommand(plugin ,"bla", [controllerId,pr2_cid, pr2_cid2,pr2_gripper],[50,3]) + +while (1): + #b = p.getJointState(pr2_gripper,2)[0] + #print("b = " + str(b)) + #p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3) + p.setGravity(0,0,-10) +p.disconnect() diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 873430e8a..f583ce29c 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4971,7 +4971,7 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, PyObject* keywds) { - static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "gearRatio", "gearAuxLink", "physicsClientId", NULL}; + static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "gearRatio", "gearAuxLink", "relativePositionTarget", "erp", "physicsClientId", NULL}; int userConstraintUniqueId = -1; b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; @@ -4985,7 +4985,9 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P double jointChildFrameOrn[4]; double maxForce = -1; double gearRatio = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOddii", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &gearRatio, &gearAuxLink, &physicsClientId)) + double relativePositionTarget=1e32; + double erp=-1; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOddiddi", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &gearRatio, &gearAuxLink, &relativePositionTarget, &erp, &physicsClientId)) { return NULL; } @@ -5007,6 +5009,16 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P { b3InitChangeUserConstraintSetFrameInB(commandHandle, jointChildFrameOrn); } + + if (relativePositionTarget<1e10) + { + b3InitChangeUserConstraintSetRelativePositionTarget(commandHandle, relativePositionTarget); + } + if (erp>=0) + { + b3InitChangeUserConstraintSetERP(commandHandle, erp); + } + if (maxForce >= 0) { b3InitChangeUserConstraintSetMaxForce(commandHandle, maxForce); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 8c28bbf4c..83521b950 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -185,7 +185,9 @@ public: virtual void setGearRatio(btScalar ratio) {} virtual void setGearAuxLink(int gearAuxLink) {} - + virtual void setRelativePositionTarget(btScalar relPosTarget){} + virtual void setErp(btScalar erp){} + }; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp index 3fdd51815..5fdb7007d 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp @@ -23,7 +23,9 @@ subject to the following restrictions: btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,1,false), m_gearRatio(1), - m_gearAuxLink(-1) + m_gearAuxLink(-1), + m_erp(0), + m_relativePositionTarget(0) { } @@ -107,9 +109,9 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& jacobianA(0)[offsetA] = 1; jacobianB(0)[offsetB] = m_gearRatio; - const btScalar posError = 0; + btScalar posError = 0; const btVector3 dummy(0, 0, 0); - btScalar erp = infoGlobal.m_erp; + btScalar kp = 1; btScalar kd = 1; int numRows = getNumRows(); @@ -129,10 +131,15 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof]; } currentVelocity += auxVel; - - //btScalar positionStabiliationTerm = erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep; - //btScalar velocityError = (m_desiredVelocity - currentVelocity); - + if (m_erp!=0) + { + btScalar currentPositionA = m_bodyA->getJointPosMultiDof(m_linkA)[dof]; + btScalar currentPositionB = m_gearRatio*m_bodyA->getJointPosMultiDof(m_linkB)[dof]; + btScalar diff = currentPositionB+currentPositionA; + btScalar desiredPositionDiff = this->m_relativePositionTarget; + posError = -m_erp*(desiredPositionDiff - diff); + } + btScalar desiredRelativeVelocity = auxVel; fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,desiredRelativeVelocity); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h index 711a73e46..0115de624 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h @@ -28,10 +28,12 @@ protected: btRigidBody* m_rigidBodyB; btVector3 m_pivotInA; btVector3 m_pivotInB; - btMatrix3x3 m_frameInA; - btMatrix3x3 m_frameInB; + btMatrix3x3 m_frameInA; + btMatrix3x3 m_frameInB; btScalar m_gearRatio; int m_gearAuxLink; + btScalar m_erp; + btScalar m_relativePositionTarget; public: @@ -102,7 +104,14 @@ public: { m_gearAuxLink = gearAuxLink; } - + virtual void setRelativePositionTarget(btScalar relPosTarget) + { + m_relativePositionTarget = relPosTarget; + } + virtual void setErp(btScalar erp) + { + m_erp = erp; + } }; #endif //BT_MULTIBODY_GEAR_CONSTRAINT_H