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