diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index a3a550062..230b4216e 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -208,6 +208,8 @@ static ExampleEntry gDefaultExamples[]= RobotControlExampleCreateFunc,ROBOT_VELOCITY_CONTROL), ExampleEntry(1,"Robot Control (PD)", "Perform some robot control tasks, using physics server and client that communicate over shared memory", RobotControlExampleCreateFunc,ROBOT_PD_CONTROL), + ExampleEntry(1,"Robot Joint Feedback", "Apply some small ping-pong target velocity jitter, and read the joint reaction forces, using physics server and client that communicate over shared memory.", + RobotControlExampleCreateFunc,ROBOT_PING_PONG_JOINT_FEEDBACK), ExampleEntry(1,"Physics Server", "Create a physics server that communicates with a physics client over shared memory", PhysicsServerCreateFunc), diff --git a/examples/SharedMemory/RobotControlExample.cpp b/examples/SharedMemory/RobotControlExample.cpp index f3dc3f39b..0ccd2640b 100644 --- a/examples/SharedMemory/RobotControlExample.cpp +++ b/examples/SharedMemory/RobotControlExample.cpp @@ -12,12 +12,14 @@ #include "../Utils/b3Clock.h" #include "PhysicsClientC_API.h" #include "../Utils/b3ResourcePath.h" +#include //const char* blaatnaam = "basename"; struct MyMotorInfo { + std::string m_jointName; btScalar m_velTarget; btScalar m_posTarget; btScalar m_kp; @@ -26,6 +28,12 @@ struct MyMotorInfo btScalar m_maxForce; int m_uIndex; int m_posIndex; + + int m_jointIndex; + btScalar m_measuredJointPosition; + btScalar m_measuredJointVelocity; + btVector3 m_measuredJointForce; + btVector3 m_measuredJointTorque; }; #define MAX_NUM_MOTORS 128 @@ -40,7 +48,7 @@ class RobotControlExample : public SharedMemoryCommon bool m_wantsShutdown; btAlignedObjectArray m_userCommandRequests; - + void createButton(const char* name, int id, bool isTrigger ); @@ -269,6 +277,25 @@ void RobotControlExample::prepareControlCommand(SharedMemoryCommand& command) } break; } + case ROBOT_PING_PONG_JOINT_FEEDBACK: + { + + command.m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_VELOCITY; + for (int i=0;i0.1) + { + m_motorTargetState[i].m_velTarget = -1.5; + } else + { + m_motorTargetState[i].m_velTarget = 1.5; + } + + b3Printf("Joint Force (Linear) [%s]=(%f,%f,%f)\n",m_motorTargetState[i].m_jointName.c_str(),m_motorTargetState[i].m_measuredJointForce.x(),m_motorTargetState[i].m_measuredJointForce.y(),m_motorTargetState[i].m_measuredJointForce.z()); + b3Printf("Joint Torque (Angular) [%s]=(%f,%f,%f)\n",m_motorTargetState[i].m_jointName.c_str(),m_motorTargetState[i].m_measuredJointTorque.x(),m_motorTargetState[i].m_measuredJointTorque.y(),m_motorTargetState[i].m_measuredJointTorque.z()); + + } + + + } + if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED) { - for (int i=0;im_jointName = info.m_jointName; + motorInfo->m_velTarget = 0.f; motorInfo->m_posTarget = 0.f; @@ -414,6 +482,7 @@ void RobotControlExample::stepSimulation(float deltaTime) char motorName[1024]; MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors]; + motorInfo->m_jointName = info.m_jointName; motorInfo->m_velTarget = 0.f; motorInfo->m_posTarget = 0.f; motorInfo->m_uIndex = info.m_uIndex; @@ -454,6 +523,31 @@ void RobotControlExample::stepSimulation(float deltaTime) m_numMotors++; break; } + case ROBOT_PING_PONG_JOINT_FEEDBACK: + { + + if (info.m_flags & JOINT_HAS_MOTORIZED_POWER) + { + if (m_numMotorsm_jointName = info.m_jointName; + motorInfo->m_velTarget = 0.f; + motorInfo->m_posTarget = 0.f; + motorInfo->m_uIndex = info.m_uIndex; + motorInfo->m_posIndex = info.m_qIndex; + motorInfo->m_jointIndex = jointIndex; + sensorCommand.m_createSensorArguments.m_jointIndex[sensorCommand.m_createSensorArguments.m_numJointSensorChanges] = jointIndex; + sensorCommand.m_createSensorArguments.m_enableJointForceSensor[sensorCommand.m_createSensorArguments.m_numJointSensorChanges] = true; + sensorCommand.m_createSensorArguments.m_numJointSensorChanges++; + m_numMotors++; + } + } + + + + break; + } default: { b3Warning("Unknown control mode in RobotControlExample::stepSimulation"); @@ -463,9 +557,17 @@ void RobotControlExample::stepSimulation(float deltaTime) } } + + + if (sensorCommand.m_createSensorArguments.m_numJointSensorChanges) + { + enqueueCommand(sensorCommand); + } + } + if (m_physicsClient.canSubmitCommand()) { if (m_userCommandRequests.size()) @@ -483,6 +585,10 @@ void RobotControlExample::stepSimulation(float deltaTime) } m_userCommandRequests.pop_back(); + if (cmd.m_type == CMD_CREATE_SENSOR) + { + b3Printf("CMD_CREATE_SENSOR!\n"); + } if (cmd.m_type == CMD_SEND_BULLET_DATA_STREAM) { char relativeFileName[1024]; @@ -537,6 +643,9 @@ void RobotControlExample::stepSimulation(float deltaTime) command.m_type =CMD_STEP_FORWARD_SIMULATION; enqueueCommand(command); + + command.m_type = CMD_REQUEST_ACTUAL_STATE; + enqueueCommand(command); } } diff --git a/examples/SharedMemory/RobotControlExample.h b/examples/SharedMemory/RobotControlExample.h index 68cdb9173..9a701b604 100644 --- a/examples/SharedMemory/RobotControlExample.h +++ b/examples/SharedMemory/RobotControlExample.h @@ -4,7 +4,8 @@ enum EnumRobotControls { ROBOT_VELOCITY_CONTROL=0, - ROBOT_PD_CONTROL=2, + ROBOT_PD_CONTROL, + ROBOT_PING_PONG_JOINT_FEEDBACK, }; class CommonExampleInterface* RobotControlExampleCreateFunc(struct CommonExampleOptions& options);