From bbc450a3de3d11a54193bba43a92d3c65b5d142e Mon Sep 17 00:00:00 2001 From: = <=> Date: Thu, 20 Aug 2015 14:57:14 -0700 Subject: [PATCH] position/velocity control through constraint for shared memory server --- examples/SharedMemory/PhysicsServer.cpp | 46 +++++++++---------- examples/SharedMemory/RobotControlExample.cpp | 43 ++++++++++++++--- 2 files changed, 60 insertions(+), 29 deletions(-) diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index e90b04676..385341789 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -103,7 +103,7 @@ struct PhysicsServerInternalData :m_sharedMemory(0), m_testBlock1(0), m_isConnected(false), - m_physicsDeltaTime(1./240.),//240.), + m_physicsDeltaTime(1./240.), m_dynamicsWorld(0), m_debugDrawer(0), m_guiHelper(0), @@ -820,30 +820,30 @@ void PhysicsServerSharedMemory::processClientCommands() if (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]; - btScalar desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; + int dof1 = 0; + 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; posIndex += mb->getLink(link).m_posVarCount; diff --git a/examples/SharedMemory/RobotControlExample.cpp b/examples/SharedMemory/RobotControlExample.cpp index deec441a8..f3dc3f39b 100644 --- a/examples/SharedMemory/RobotControlExample.cpp +++ b/examples/SharedMemory/RobotControlExample.cpp @@ -20,6 +20,9 @@ struct MyMotorInfo { btScalar m_velTarget; btScalar m_posTarget; + btScalar m_kp; + btScalar m_kd; + btScalar m_maxForce; int m_uIndex; int m_posIndex; @@ -253,9 +256,9 @@ void RobotControlExample::prepareControlCommand(SharedMemoryCommand& command) int uIndex = m_motorTargetState[i].m_uIndex; - command.m_sendDesiredStateCommandArgument.m_Kp[uIndex] = 10; - command.m_sendDesiredStateCommandArgument.m_Kd[uIndex] = 2; - command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[uIndex] = 1000;//max force + command.m_sendDesiredStateCommandArgument.m_Kp[uIndex] = m_motorTargetState[i].m_kp; + command.m_sendDesiredStateCommandArgument.m_Kd[uIndex] = m_motorTargetState[i].m_kd; + command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[uIndex] = 10000;//max force btScalar targetVel = m_motorTargetState[i].m_velTarget; command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel; @@ -396,6 +399,7 @@ void RobotControlExample::stepSimulation(float deltaTime) MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors]; motorInfo->m_velTarget = 0.f; motorInfo->m_posTarget = 0.f; + motorInfo->m_uIndex = info.m_uIndex; SliderParams slider(motorName,&motorInfo->m_velTarget); @@ -408,18 +412,45 @@ void RobotControlExample::stepSimulation(float deltaTime) case ROBOT_PD_CONTROL: { char motorName[1024]; - sprintf(motorName,"%s q", info.m_jointName); + MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors]; motorInfo->m_velTarget = 0.f; motorInfo->m_posTarget = 0.f; motorInfo->m_uIndex = info.m_uIndex; 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); slider.m_minVal=-SIMD_PI; slider.m_maxVal=SIMD_PI; 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++; break; }