From 3f60be59ad0ea16dc02e98f53098c5d6f90dd0f8 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 20 Nov 2017 12:35:36 -0800 Subject: [PATCH 1/2] bump up pybullet version to 1.7.0 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index a3ad02b43..fd26597bb 100644 --- a/setup.py +++ b/setup.py @@ -441,7 +441,7 @@ print("-----") setup( name = 'pybullet', - version='1.6.9', + version='1.7.0', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', From ab843b26f06a3f91ebd13d11f1b3c134c55edd66 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 21 Nov 2017 17:05:28 -0800 Subject: [PATCH 2/2] expose a maximum velocity due to the joint motor in position control. see also pybullet/examples/motorMaxVelocity.py this fixes issue 1444 --- examples/SharedMemory/PhysicsClientC_API.cpp | 13 +++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 2 ++ .../SharedMemory/PhysicsServerCommandProcessor.cpp | 4 ++++ examples/SharedMemory/SharedMemoryCommands.h | 4 +++- examples/pybullet/examples/motorMaxVelocity.py | 12 ++++++++++++ examples/pybullet/pybullet.c | 11 ++++++++--- 6 files changed, 42 insertions(+), 4 deletions(-) create mode 100644 examples/pybullet/examples/motorMaxVelocity.py diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 7b685974f..36e5d4330 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -606,6 +606,19 @@ B3_SHARED_API int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, return 0; } +B3_SHARED_API int b3JointControlSetMaximumVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double maximumVelocity) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + if ((dofIndex>=0) && (dofIndex < MAX_DEGREE_OF_FREEDOM)) + { + command->m_sendDesiredStateCommandArgument.m_rhsClamp[dofIndex] = maximumVelocity; + command->m_updateFlags |= SIM_DESIRED_STATE_HAS_RHS_CLAMP; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_RHS_CLAMP; + } + return 0; +} + B3_SHARED_API int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index f3bef30e5..63bc958cf 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -367,6 +367,8 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit2(b3PhysicsC B3_SHARED_API int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value); B3_SHARED_API int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); B3_SHARED_API int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); +B3_SHARED_API int b3JointControlSetMaximumVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double maximumVelocity); + ///Only use when controlMode is CONTROL_MODE_VELOCITY B3_SHARED_API int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */ diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index b96177ddc..5c1fafb71 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4531,6 +4531,10 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct if (motor) { + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex]&SIM_DESIRED_STATE_HAS_RHS_CLAMP)!=0) + { + motor->setRhsClamp(clientCmd.m_sendDesiredStateCommandArgument.m_rhsClamp[velIndex]); + } bool hasDesiredPosOrVel = false; btScalar kp = 0.f; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 388a472d7..c16733e45 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -358,10 +358,11 @@ struct SendDesiredStateArgs { int m_bodyUniqueId; int m_controlMode; - + //PD parameters in case m_controlMode == CONTROL_MODE_POSITION_VELOCITY_PD double m_Kp[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link double m_Kd[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link + double m_rhsClamp[MAX_DEGREE_OF_FREEDOM]; int m_hasDesiredStateFlags[MAX_DEGREE_OF_FREEDOM]; @@ -389,6 +390,7 @@ enum EnumSimDesiredStateUpdateFlags SIM_DESIRED_STATE_HAS_KD=4, SIM_DESIRED_STATE_HAS_KP=8, SIM_DESIRED_STATE_HAS_MAX_FORCE=16, + SIM_DESIRED_STATE_HAS_RHS_CLAMP=32 }; diff --git a/examples/pybullet/examples/motorMaxVelocity.py b/examples/pybullet/examples/motorMaxVelocity.py new file mode 100644 index 000000000..fad9e2fd4 --- /dev/null +++ b/examples/pybullet/examples/motorMaxVelocity.py @@ -0,0 +1,12 @@ +import pybullet as p +import time + +p.connect(p.GUI) +cartpole=p.loadURDF("cartpole.urdf") +p.setRealTimeSimulation(1) +p.setJointMotorControl2(cartpole,1,p.POSITION_CONTROL,targetPosition=1000,targetVelocity=0,force=1000, positionGain=1, velocityGain=0, maxVelocity=0.5) +while (1): + p.setGravity(0,0,-10) + js = p.getJointState(cartpole,1) + print("position=",js[0],"velocity=",js[1]) + time.sleep(0.01) \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b11fcdc25..ade0ec1c8 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1748,12 +1748,13 @@ static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args, double force = 100000.0; double kp = 0.1; double kd = 1.0; + double maxVelocity = -1; b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char* kwlist[] = {"bodyUniqueId", "jointIndex", "controlMode", "targetPosition", "targetVelocity", "force", "positionGain", "velocityGain", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|dddddi", kwlist, &bodyUniqueId, &jointIndex, &controlMode, - &targetPosition, &targetVelocity, &force, &kp, &kd, &physicsClientId)) + static char* kwlist[] = {"bodyUniqueId", "jointIndex", "controlMode", "targetPosition", "targetVelocity", "force", "positionGain", "velocityGain", "maxVelocity", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|ddddddi", kwlist, &bodyUniqueId, &jointIndex, &controlMode, + &targetPosition, &targetVelocity, &force, &kp, &kd, &maxVelocity, &physicsClientId)) { //backward compatibility, bodyIndex -> bodyUniqueId, don't need to update this function: people have to migrate to bodyUniqueId static char* kwlist2[] = {"bodyIndex", "jointIndex", "controlMode", "targetPosition", "targetVelocity", "force", "positionGain", "velocityGain", "physicsClientId", NULL}; @@ -1816,6 +1817,10 @@ static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args, case CONTROL_MODE_POSITION_VELOCITY_PD: { + if (maxVelocity>0) + { + b3JointControlSetMaximumVelocity(commandHandle, info.m_uIndex, maxVelocity); + } b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition); b3JointControlSetKp(commandHandle, info.m_uIndex, kp);