AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-15 14:10:11 +00:00
Code Issues Releases Wiki Activity

Merge pull request #1449 from erwincoumans/master

Browse Source 
expose a maximum velocity due to the joint motor in position control
This commit is contained in:
erwincoumans 2017-11-21 19:45:14 -08:00 committed by GitHub
commit 4ee9a17d6d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 43 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -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;

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -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 */

4
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -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;

4
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -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
};

12
examples/pybullet/examples/motorMaxVelocity.py Normal file
View File

@ -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)

11
examples/pybullet/pybullet.c
View File

@ -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);

2
setup.py
View File

@ -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',