{"action":"created","issue":{"url":"https://api.github.com/repos/bulletphysics/bullet3/issues/1459","repository_url":"https://api.github.com/repos/bulletphysics/bullet3","labels_url":"https://api.github.com/repos/bulletphysics/bullet3/issues/1459/labels{/name}","comments_url":"https://api.github.com/repos/bulletphysics/bullet3/issues/1459/comments","events_url":"https://api.github.com/repos/bulletphysics/bullet3/issues/1459/events","html_url":"https://github.com/bulletphysics/bullet3/issues/1459","id":276794518,"number":1459,"title":"Using CONTROL_MODE_TORQUE with the C API","user":{"login":"avikde","id":3183687,"avatar_url":"https://avatars0.githubusercontent.com/u/3183687?v=4","gravatar_id":"","url":"https://api.github.com/users/avikde","html_url":"https://github.com/avikde","followers_url":"https://api.github.com/users/avikde/followers","following_url":"https://api.github.com/users/avikde/following{/other_user}","gists_url":"https://api.github.com/users/avikde/gists{/gist_id}","starred_url":"https://api.github.com/users/avikde/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/avikde/subscriptions","organizations_url":"https://api.github.com/users/avikde/orgs","repos_url":"https://api.github.com/users/avikde/repos","events_url":"https://api.github.com/users/avikde/events{/privacy}","received_events_url":"https://api.github.com/users/avikde/received_events","type":"User","site_admin":false},"labels":[],"state":"open","locked":false,"assignee":null,"assignees":[],"milestone":null,"comments":4,"created_at":"2017-11-26T02:44:19Z","updated_at":"2017-11-26T04:44:35Z","closed_at":null,"author_association":"NONE","body":"I'm having some trouble using CONTROL_MODE_TORQUE to command torques to joints with the C API. I'm trying to create a fairly minimal example that can be used to \"calibrate\" the simulation to ground truth (using a MATLAB/Mathematica simulation). It looks like `TwoJointRobot_wo_fixedJoints.urdf` has all the inertial values populated and can be used for this.\r\n\r\nMy code:\r\n```cpp\r\n\tcommand = b3LoadUrdfCommandInit(kPhysClient, \"TwoJointRobot_wo_fixedJoints.urdf\");\r\n\tb3LoadUrdfCommandSetStartPosition(command, -2, 0, 1);\r\n\tb3LoadUrdfCommandSetUseMultiBody(command, true);\r\n\tstatusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);\r\n\tstatusType = b3GetStatusType(statusHandle);\r\n\tb3Assert(statusType == CMD_URDF_LOADING_COMPLETED);\r\n\tif (statusType == CMD_URDF_LOADING_COMPLETED) {\r\n\t\ttwojoint = b3GetStatusBodyIndex(statusHandle);\r\n\t}\r\n\tint numJoints = b3GetNumJoints(kPhysClient, twojoint);\r\n\tprintf(\"twojoint numjoints = %d\\n\", numJoints);\r\n\t// Loop through all joints\r\n\tfor (int i=0; i<numJoints; ++i) {\r\n\t\tb3GetJointInfo(kPhysClient, twojoint, i, &jointInfo);\r\n\t\tif (jointInfo.m_jointName[0]) {\r\n\t\t\tjointNameToId[std::string(jointInfo.m_jointName)] = i;\r\n\t\t} else {\r\n\t\t\tcontinue;\r\n\t\t}\r\n\t\t// max force = 0\r\n\t\tcommand = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_VELOCITY);\r\n\t\tb3JointControlSetMaximumForce(command, jointInfo.m_uIndex, 0);\r\n\t\tstatusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);\r\n\t}\r\n\r\n\tunsigned long dtus1 = (unsigned long) 1000000.0*FIXED_TIMESTEP;\r\n\tdouble simTimeUS1 = 0;\r\n\tdouble zer[3] = {0,0,0};\r\n\tdouble q[2], v[2];\r\n\twhile (b3CanSubmitCommand(kPhysClient)) {\r\n\t\tsimTimeUS1 += dtus1;\r\n\t\t// reset base velocity\r\n\t\tcommand = b3CreatePoseCommandInit(kPhysClient, twojoint);\r\n\t\tb3CreatePoseCommandSetBaseLinearVelocity(command, zer);\r\n\t\tb3CreatePoseCommandSetBaseAngularVelocity(command, zer);\r\n\t\tstatusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);\r\n\t\t// apply some torque\r\n\t\tb3GetJointInfo(kPhysClient, twojoint, jointNameToId[\"joint_2\"], &jointInfo);\r\n\t\t// command = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_VELOCITY);\r\n\t\t// b3JointControlSetDesiredVelocity(command, jointInfo.m_uIndex, 1);\r\n\t\tcommand = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_TORQUE);\r\n\t\tb3JointControlSetDesiredForceTorque(command, jointInfo.m_uIndex, 200);\r\n\t\t// b3JointControlSetMaximumForce(command, jointInfo.m_uIndex, 100);\r\n\t\tstatusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);\r\n\r\n\t\t// get joint values\r\n\t\tcommand = b3RequestActualStateCommandInit(kPhysClient, twojoint);\r\n\t\tstatusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);\r\n\t\tb3GetJointState(kPhysClient, statusHandle, jointNameToId[\"joint_1\"], &state);\r\n\t\tq[0] = state.m_jointPosition;\r\n\t\tv[0] = state.m_jointVelocity;\r\n\t\tb3GetJointState(kPhysClient, statusHandle, jointNameToId[\"joint_2\"], &state);\r\n\t\tq[1] = state.m_jointPosition;\r\n\t\tv[1] = state.m_jointVelocity;\r\n\r\n\t\tstatusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, b3InitStepSimulationCommand(kPhysClient));\r\n\r\n\t\t// debugging output\r\n\t\tprintf(\"%.3f\\t%.3f\\t%.3f\\t%.3f\\t%.3f\\n\", 0.000001*simTimeUS1, q[0], q[1], v[0], v[1]);\r\n\t\tstd::this_thread::sleep_for(std::chrono::microseconds(dtus1));\r\n\t}\r\n```\r\n\r\nDoes this usage look correct? When I use the two lines with `CONTROL_MODE_VELOCITY` the behavior seems as expected (more or less), but when substituting the `CONTROL_MODE_TORQUE` lines there is no motion at all. BTW it seems like `b3JointControlSetDesiredForceTorque` and `b3JointControlSetMaximumForce` do exactly the same thing, so I'm just using the former in `CONTROL_MODE_TORQUE`."},"comment":{"url":"https://api.github.com/repos/bulletphysics/bullet3/issues/comments/346983821","html_url":"https://github.com/bulletphysics/bullet3/issues/1459#issuecomment-346983821","issue_url":"https://api.github.com/repos/bulletphysics/bullet3/issues/1459","id":346983821,"user":{"login":"erwincoumans","id":725468,"avatar_url":"https://avatars3.githubusercontent.com/u/725468?v=4","gravatar_id":"","url":"https://api.github.com/users/erwincoumans","html_url":"https://github.com/erwincoumans","followers_url":"https://api.github.com/users/erwincoumans/followers","following_url":"https://api.github.com/users/erwincoumans/following{/other_user}","gists_url":"https://api.github.com/users/erwincoumans/gists{/gist_id}","starred_url":"https://api.github.com/users/erwincoumans/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/erwincoumans/subscriptions","organizations_url":"https://api.github.com/users/erwincoumans/orgs","repos_url":"https://api.github.com/users/erwincoumans/repos","events_url":"https://api.github.com/users/erwincoumans/events{/privacy}","received_events_url":"https://api.github.com/users/erwincoumans/received_events","type":"User","site_admin":false},"created_at":"2017-11-26T04:44:35Z","updated_at":"2017-11-26T04:44:35Z","author_association":"OWNER","body":"The C-API is very error-prone and verbose, mainly for pybullet and C++ binding.\r\n\r\nYou can 'fix' the base of the body using\r\n b3LoadUrdfCommandSetUseFixedBase(command,true);\r\n\r\nYou need to use b3JointControlSetDesiredVelocity together with b3JointControlSetMaximumForce, otherwise the command has no effect.\r\n ```\r\ncommand = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_VELOCITY);\r\n b3JointControlSetDesiredVelocity(command,jointInfo.m_uIndex,0);\r\n b3JointControlSetMaximumForce(command, jointInfo.m_uIndex, 0);\r\n\r\n```\r\n\r\nHere is a working version. Note that the links move very fast and collide with eachother soon.\r\n\r\n```\r\n\r\n#include <map>\r\n#include <string>\r\n#include <chrono>\r\n#include <thread>\r\n\r\n#include \"SharedMemory/PhysicsClientC_API.h\"\r\n#include \"Bullet3Common/b3Vector3.h\"\r\n#include \"Bullet3Common/b3Quaternion.h\"\r\n\r\nextern const int CONTROL_RATE;\r\nconst int CONTROL_RATE = 250;\r\n\r\n// Bullet globals\r\nb3PhysicsClientHandle kPhysClient = 0;\r\nconst b3Scalar FIXED_TIMESTEP = 1.0/((b3Scalar)CONTROL_RATE);\r\n// temp vars used a lot\r\nb3SharedMemoryCommandHandle command;\r\nb3SharedMemoryStatusHandle statusHandle;\r\nint statusType, ret;\r\nb3JointInfo jointInfo;\r\nb3JointSensorState state;\r\n// test\r\nint twojoint;\r\nstd::map< std::string, int > jointNameToId;\r\n\r\nint main(int argc, char* argv[]) {\r\n    kPhysClient = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);\r\n    if (!kPhysClient)\r\n        return -1;\r\n    // visualizer\r\n    command = b3InitConfigureOpenGLVisualizer(kPhysClient);\r\n    b3ConfigureOpenGLVisualizerSetVisualizationFlags(command, COV_ENABLE_GUI, 0);\r\n    b3SubmitClientCommandAndWaitStatus(kPhysClient, command);\r\n    b3ConfigureOpenGLVisualizerSetVisualizationFlags(command, COV_ENABLE_SHADOWS, 0);\r\n    b3SubmitClientCommandAndWaitStatus(kPhysClient, command);\r\n    \r\n    b3SetTimeOut(kPhysClient, 10);\r\n    \r\n    //syncBodies is only needed when connecting to an existing physics server that has already some bodies\r\n    command = b3InitSyncBodyInfoCommand(kPhysClient);\r\n    statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);\r\n    statusType = b3GetStatusType(statusHandle);\r\n    \r\n    // set fixed time step\r\n    command = b3InitPhysicsParamCommand(kPhysClient);\r\n    ret = b3PhysicsParamSetTimeStep(command, FIXED_TIMESTEP);\r\n    statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);\r\n    ret = b3PhysicsParamSetRealTimeSimulation(command, false);\r\n    statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);\r\n    \r\n    b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);\r\n    \r\n    \r\n    // load test\r\n    command = b3LoadUrdfCommandInit(kPhysClient, \"TwoJointRobot_wo_fixedJoints.urdf\");\r\n    b3LoadUrdfCommandSetStartPosition(command, -2, 0, 1);\r\n    b3LoadUrdfCommandSetUseFixedBase(command,true);\r\n    // q.setEulerZYX(0, 0, 0);\r\n    // b3LoadUrdfCommandSetStartOrientation(command, q[0], q[1], q[2], q[3]);\r\n    b3LoadUrdfCommandSetUseMultiBody(command, true);\r\n    statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);\r\n    statusType = b3GetStatusType(statusHandle);\r\n    b3Assert(statusType == CMD_URDF_LOADING_COMPLETED);\r\n    if (statusType == CMD_URDF_LOADING_COMPLETED) {\r\n        twojoint = b3GetStatusBodyIndex(statusHandle);\r\n    }\r\n    int numJoints = b3GetNumJoints(kPhysClient, twojoint);\r\n    printf(\"twojoint numjoints = %d\\n\", numJoints);\r\n    // Loop through all joints\r\n    for (int i=0; i<numJoints; ++i) {\r\n        // max force = 0\r\n        command = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_VELOCITY);\r\n        b3JointControlSetDesiredVelocity(command,jointInfo.m_uIndex,0);\r\n        b3JointControlSetMaximumForce(command, jointInfo.m_uIndex, 0);\r\n        statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);\r\n        \r\n        b3GetJointInfo(kPhysClient, twojoint, i, &jointInfo);\r\n        if (jointInfo.m_jointName[0]) {\r\n            jointNameToId[std::string(jointInfo.m_jointName)] = i;\r\n        } else {\r\n            continue;\r\n        }\r\n        \r\n    }\r\n    \r\n    // loop\r\n    unsigned long dtus1 = (unsigned long) 1000000.0*FIXED_TIMESTEP;\r\n    double simTimeUS1 = 0;\r\n    double zer[3] = {0,0,0};\r\n    double q[2], v[2];\r\n    while (b3CanSubmitCommand(kPhysClient)) {\r\n        simTimeUS1 += dtus1;\r\n        // reset base velocity\r\n        // apply some torque\r\n        b3GetJointInfo(kPhysClient, twojoint, jointNameToId[\"joint_2\"], &jointInfo);\r\n        // command = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_VELOCITY);\r\n        // b3JointControlSetDesiredVelocity(command, jointInfo.m_uIndex, 1);\r\n        command = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_TORQUE);\r\n        b3JointControlSetDesiredForceTorque(command, jointInfo.m_uIndex, 200);\r\n        // b3JointControlSetMaximumForce(command, jointInfo.m_uIndex, 100);\r\n        statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);\r\n      \r\n        statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, b3InitStepSimulationCommand(kPhysClient));\r\n        \r\n        // get joint values\r\n        command = b3RequestActualStateCommandInit(kPhysClient, twojoint);\r\n        statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);\r\n        b3GetJointState(kPhysClient, statusHandle, jointNameToId[\"joint_1\"], &state);\r\n        q[0] = state.m_jointPosition;\r\n        v[0] = state.m_jointVelocity;\r\n        b3GetJointState(kPhysClient, statusHandle, jointNameToId[\"joint_2\"], &state);\r\n        q[1] = state.m_jointPosition;\r\n        v[1] = state.m_jointVelocity;\r\n        \r\n        statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, b3InitStepSimulationCommand(kPhysClient));\r\n        \r\n        // debugging output\r\n        printf(\"%.3f\\t%.3f\\t%.3f\\t%.3f\\t%.3f\\n\", 0.000001*simTimeUS1, q[0], q[1], v[0], v[1]);\r\n        std::this_thread::sleep_for(std::chrono::microseconds(dtus1));\r\n    }\r\n    b3DisconnectSharedMemory(kPhysClient);\r\n    return 0;\r\n}\r\n```\r\n"}}
{"actor":{"display_login":"erwincoumans"}}