Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

expose a maximum velocity due to the joint motor in position control.

Browse Source 
see also pybullet/examples/motorMaxVelocity.py
this fixes issue 1444
This commit is contained in:
erwincoumans
2017-11-21 17:05:28 -08:00
parent 3f60be59ad
commit ab843b26f0
6 changed files with 42 additions and 4 deletions
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);