void prepareControlCommand(b3SharedMemoryCommandHandle commandHandle) { for (int i=0;i<m_numMotors;i++) { btScalar targetPos = m_motorTargetPositions[i].m_posTarget; int qIndex = m_motorTargetPositions[i].m_qIndex; int uIndex = m_motorTargetPositions[i].m_uIndex; b3JointControlSetDesiredPosition(commandHandle, qIndex, targetPos); b3JointControlSetKp(commandHandle, uIndex, 0.1); b3JointControlSetMaximumForce(commandHandle,uIndex,1000); } }
void prepareControlCommand(b3SharedMemoryCommandHandle commandHandle) { for (int i = 0; i < m_numMotors; i++) { btScalar targetPos = m_motorTargetPositions[i].m_posTarget; int qIndex = m_motorTargetPositions[i].m_qIndex; int uIndex = m_motorTargetPositions[i].m_uIndex; static int serial = 0; serial++; // b3Printf("# motors = %d, cmd[%d] qIndex = %d, uIndex = %d, targetPos = %f", m_numMotors, serial, qIndex,uIndex,targetPos); b3JointControlSetDesiredPosition(commandHandle, qIndex, targetPos); b3JointControlSetDesiredVelocity(commandHandle, uIndex, 0); b3JointControlSetKp(commandHandle, qIndex, 0.2); b3JointControlSetKd(commandHandle, uIndex, 1.); b3JointControlSetMaximumForce(commandHandle, uIndex, 5000); } }
B3_SHARED_API int preTickPluginCallback_vrSyncPlugin(struct b3PluginContext* context) { MyClass* obj = (MyClass*)context->m_userPointer; if (obj && obj->m_controllerId >= 0) { { int i = 0; { for (int n = 0; n < context->m_numVRControllerEvents; n++) { const b3VRControllerEvent& event = context->m_vrControllerEvents[n]; if (event.m_controllerId == obj->m_controllerId) { if (obj->m_constraintId >= 0) { struct b3UserConstraint constraintInfo; if (b3GetUserConstraintInfo(context->m_physClient, obj->m_constraintId, &constraintInfo)) { //this is basically equivalent to doing this in Python/pybullet: //p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=...) b3SharedMemoryCommandHandle commandHandle; int userConstraintUniqueId = obj->m_constraintId; commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, userConstraintUniqueId); double pos[4] = {event.m_pos[0], event.m_pos[1], event.m_pos[2], 1}; b3InitChangeUserConstraintSetPivotInB(commandHandle, pos); double orn[4] = {event.m_orn[0], event.m_orn[1], event.m_orn[2], event.m_orn[3]}; b3InitChangeUserConstraintSetFrameInB(commandHandle, orn); b3InitChangeUserConstraintSetMaxForce(commandHandle, obj->m_maxForce); b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle); } } // apply the analogue button to close the constraint, using a gear constraint with position target if (obj->m_constraintId2 >= 0) { struct b3UserConstraint constraintInfo; if (b3GetUserConstraintInfo(context->m_physClient, obj->m_constraintId2, &constraintInfo)) { //this block is similar to //p.changeConstraint(c,gearRatio=1, erp=..., relativePositionTarget=relPosTarget, maxForce=...) //printf("obj->m_constraintId2=%d\n", obj->m_constraintId2); b3SharedMemoryCommandHandle commandHandle; commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, obj->m_constraintId2); //0 -> open, 1 = closed double openPos = 1.; double relPosTarget = openPos - (event.m_analogAxis * openPos); b3InitChangeUserConstraintSetRelativePositionTarget(commandHandle, relPosTarget); b3InitChangeUserConstraintSetERP(commandHandle, 1); b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle); } } //printf("event.m_analogAxis=%f\n", event.m_analogAxis); // use the pr2_gripper motors to keep the gripper centered/symmetric around the center axis if (obj->m_gripperId >= 0) { //this block is similar to //b = p.getJointState(pr2_gripper,2)[0] //print("b = " + str(b)) //p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3) //printf("obj->m_gripperId=%d\n", obj->m_gripperId); { b3SharedMemoryCommandHandle cmd_handle = b3RequestActualStateCommandInit(context->m_physClient, obj->m_gripperId); b3SharedMemoryStatusHandle status_handle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, cmd_handle); int status_type = b3GetStatusType(status_handle); if (status_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED) { //printf("status_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED\n"); b3JointSensorState sensorState; if (b3GetJointState(context->m_physClient, status_handle, 2, &sensorState)) { b3SharedMemoryCommandHandle commandHandle; double targetPosition = sensorState.m_jointPosition; //printf("targetPosition =%f\n", targetPosition); if (1) { b3JointInfo info; b3GetJointInfo(context->m_physClient, obj->m_gripperId, 0, &info); commandHandle = b3JointControlCommandInit2(context->m_physClient, obj->m_gripperId, CONTROL_MODE_POSITION_VELOCITY_PD); double kp = .1; double targetVelocity = 0; double kd = .6; b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition); b3JointControlSetKp(commandHandle, info.m_uIndex, kp); b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity); b3JointControlSetKd(commandHandle, info.m_uIndex, kd); b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, obj->m_maxForce2); b3SubmitClientCommandAndWaitStatus(context->m_physClient, cmd_handle); } } else { //printf("???\n"); } } else { //printf("no\n"); } } } } } } } } return 0; }