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

position/velocity control through constraint for shared memory server

Browse Source
This commit is contained in:
=
2015-08-20 14:57:14 -07:00
parent 63873e2926
commit bbc450a3de
2 changed files with 60 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -103,7 +103,7 @@ struct PhysicsServerInternalData
:m_sharedMemory(0), :m_sharedMemory(0),
m_testBlock1(0), m_testBlock1(0),
m_isConnected(false), m_isConnected(false),
m_physicsDeltaTime(1./240.),//240.), m_physicsDeltaTime(1./240.),
m_dynamicsWorld(0), m_dynamicsWorld(0),
m_debugDrawer(0), m_debugDrawer(0),
m_guiHelper(0), m_guiHelper(0),
@@ -820,30 +820,30 @@ void PhysicsServerSharedMemory::processClientCommands()
if (motorPtr) if (motorPtr)
{ {
btMultiBodyJointMotor* motor = *motorPtr; btMultiBodyJointMotor* motor = *motorPtr;
motor->setMaxAppliedImpulse(0);
} btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex];
btScalar desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex];
btScalar kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex];
btScalar kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex];
btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex]; int dof1 = 0;
btScalar desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; btScalar currentPosition = mb->getJointPosMultiDof(link)[dof1];
btScalar currentVelocity = mb->getJointVelMultiDof(link)[dof1];
btScalar positionStabiliationTerm = (desiredPosition-currentPosition)/m_data->m_physicsDeltaTime;
btScalar velocityError = (desiredVelocity - currentVelocity);
desiredVelocity = kp * positionStabiliationTerm +
kd * velocityError;
motor->setVelocityTarget(desiredVelocity);
btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(1000);//maxImp);
numMotors++;
}
btScalar maxForce = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex];
btScalar kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex];
btScalar kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex];
int dof1 = 0;
btScalar qActual = mb->getJointPosMultiDof(link)[dof1];
btScalar qdActual = mb->getJointVelMultiDof(link)[dof1];
btScalar positionError = (desiredPosition-qActual);
btScalar velocityError = (desiredVelocity-qdActual);
btScalar force = kp * positionError + kd*velocityError;
btClamp(force,-maxForce,maxForce);
if (m_data->m_verboseOutput)
{
b3Printf("Apply force %f (kp=%f, kd=%f at link %d\n", force,kp,kd,link);
}
mb->addJointTorque(link, force);//we assume we have 1-DOF motors only at the moment
//mb->addJointTorqueMultiDof(link,&force);
numMotors++;
} }
velIndex += mb->getLink(link).m_dofCount; velIndex += mb->getLink(link).m_dofCount;
posIndex += mb->getLink(link).m_posVarCount; posIndex += mb->getLink(link).m_posVarCount;

43
examples/SharedMemory/RobotControlExample.cpp
View File

@@ -20,6 +20,9 @@ struct MyMotorInfo
{ {
btScalar m_velTarget; btScalar m_velTarget;
btScalar m_posTarget; btScalar m_posTarget;
btScalar m_kp;
btScalar m_kd;
btScalar m_maxForce; btScalar m_maxForce;
int m_uIndex; int m_uIndex;
int m_posIndex; int m_posIndex;
@@ -253,9 +256,9 @@ void RobotControlExample::prepareControlCommand(SharedMemoryCommand& command)
int uIndex = m_motorTargetState[i].m_uIndex; int uIndex = m_motorTargetState[i].m_uIndex;
command.m_sendDesiredStateCommandArgument.m_Kp[uIndex] = 10; command.m_sendDesiredStateCommandArgument.m_Kp[uIndex] = m_motorTargetState[i].m_kp;
command.m_sendDesiredStateCommandArgument.m_Kd[uIndex] = 2; command.m_sendDesiredStateCommandArgument.m_Kd[uIndex] = m_motorTargetState[i].m_kd;
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[uIndex] = 1000;//max force command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[uIndex] = 10000;//max force
btScalar targetVel = m_motorTargetState[i].m_velTarget; btScalar targetVel = m_motorTargetState[i].m_velTarget;
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel; command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel;
@@ -396,6 +399,7 @@ void RobotControlExample::stepSimulation(float deltaTime)
MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors]; MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
motorInfo->m_velTarget = 0.f; motorInfo->m_velTarget = 0.f;
motorInfo->m_posTarget = 0.f; motorInfo->m_posTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex; motorInfo->m_uIndex = info.m_uIndex;
SliderParams slider(motorName,&motorInfo->m_velTarget); SliderParams slider(motorName,&motorInfo->m_velTarget);
@@ -408,18 +412,45 @@ void RobotControlExample::stepSimulation(float deltaTime)
case ROBOT_PD_CONTROL: case ROBOT_PD_CONTROL:
{ {
char motorName[1024]; char motorName[1024];
sprintf(motorName,"%s q", info.m_jointName);
MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors]; MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
motorInfo->m_velTarget = 0.f; motorInfo->m_velTarget = 0.f;
motorInfo->m_posTarget = 0.f; motorInfo->m_posTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex; motorInfo->m_uIndex = info.m_uIndex;
motorInfo->m_posIndex = info.m_qIndex; motorInfo->m_posIndex = info.m_qIndex;
motorInfo->m_kp = 1;
motorInfo->m_kd = 0;
{
sprintf(motorName,"%s kp", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_kp);
slider.m_minVal=0;
slider.m_maxVal=1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
sprintf(motorName,"%s q", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_posTarget); SliderParams slider(motorName,&motorInfo->m_posTarget);
slider.m_minVal=-SIMD_PI; slider.m_minVal=-SIMD_PI;
slider.m_maxVal=SIMD_PI; slider.m_maxVal=SIMD_PI;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
sprintf(motorName,"%s kd", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_kd);
slider.m_minVal=0;
slider.m_maxVal=1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
sprintf(motorName,"%s q'", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_velTarget);
slider.m_minVal=-10;
slider.m_maxVal=10;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
m_numMotors++; m_numMotors++;
break; break;
} }