diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 813457225..434a99b7f 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -179,6 +179,15 @@ int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandH return 0; } +int b3PhysicsParamSetInternalSimFlags(b3SharedMemoryCommandHandle commandHandle, int flags) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + command->m_physSimParamArgs.m_internalSimFlags = flags; + command->m_updateFlags |= SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS; + return 0; +} + int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 2d2a4460c..68e579f6f 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -108,6 +108,9 @@ int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation); int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations); +//b3PhysicsParamSetInternalSimFlags is for internal/temporary/easter-egg/experimental demo purposes +//Use at own risk: magic things may or my not happen when calling this API +int b3PhysicsParamSetInternalSimFlags(b3SharedMemoryCommandHandle commandHandle, int flags); b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 613bc0234..667179239 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -41,7 +41,8 @@ btVector3 gLastPickPos(0, 0, 0); bool gCloseToKuka=false; bool gEnableRealTimeSimVR=false; -bool gCreateSamuraiRobotAssets = true; +bool gCreateDefaultRobotAssets = false; +int gInternalSimFlags = 0; int gCreateObjectSimVR = -1; btScalar simTimeScalingFactor = 1; @@ -2165,6 +2166,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { m_data->m_allowRealTimeSimulation = clientCmd.m_physSimParamArgs.m_allowRealTimeSimulation; } + + //see + if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS) + { + //these flags are for internal/temporary/easter-egg/experimental demo purposes, use at own risk + gCreateDefaultRobotAssets = (clientCmd.m_physSimParamArgs.m_internalSimFlags & SIM_PARAM_INTERNAL_CREATE_ROBOT_ASSETS); + gInternalSimFlags = clientCmd.m_physSimParamArgs.m_internalSimFlags; + } + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_GRAVITY) { btVector3 grav(clientCmd.m_physSimParamArgs.m_gravityAcceleration[0], @@ -3224,474 +3234,11 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) { if ((gEnableRealTimeSimVR || m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper) { - static btAlignedObjectArray gBufferServerToClient; - gBufferServerToClient.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - int bodyId = 0; - - if (gCreateObjectSimVR >= 0) - { - gCreateObjectSimVR = -1; - btMatrix3x3 mat(gVRGripperOrn); - btScalar spawnDistance = 0.1; - btVector3 spawnDir = mat.getColumn(0); - btVector3 shiftPos = spawnDir*spawnDistance; - btVector3 spawnPos = gVRGripperPos + shiftPos; - loadUrdf("sphere_small.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - //loadUrdf("lego/lego.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - m_data->m_sphereId = bodyId; - InteralBodyData* parentBody = m_data->getHandle(bodyId); - if (parentBody->m_multiBody) - { - parentBody->m_multiBody->setBaseVel(spawnDir * 5); - } - } - ///this hardcoded C++ scene creation is temporary for demo purposes. It will be done in Python later... - if (gCreateSamuraiRobotAssets) + if (gCreateDefaultRobotAssets) { - if (!m_data->m_hasGround) - { - m_data->m_hasGround = true; - - loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("samurai.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - - if (m_data->m_gripperRigidbodyFixed == 0) - { - int bodyId = 0; - - if (loadUrdf("pr2_gripper.urdf", btVector3(0, 0, 0.1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size())) - { - InteralBodyData* parentBody = m_data->getHandle(bodyId); - if (parentBody->m_multiBody) - { - parentBody->m_multiBody->setHasSelfCollision(0); - btVector3 pivotInParent(0.2, 0, 0); - btMatrix3x3 frameInParent; - //frameInParent.setRotation(btQuaternion(0, 0, 0, 1)); - frameInParent.setIdentity(); - btVector3 pivotInChild(0, 0, 0); - btMatrix3x3 frameInChild; - frameInChild.setIdentity(); - - m_data->m_gripperRigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody, -1, 0, pivotInParent, pivotInChild, frameInParent, frameInChild); - m_data->m_gripperMultiBody = parentBody->m_multiBody; - if (m_data->m_gripperMultiBody->getNumLinks() > 2) - { - m_data->m_gripperMultiBody->setJointPos(0, 0); - m_data->m_gripperMultiBody->setJointPos(2, 0); - } - m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(500); - btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld; - world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed); - } - } - } - - loadUrdf("kuka_iiwa/model_vr_limits.urdf", btVector3(1.4, -0.2, 0.6), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - m_data->m_KukaId = bodyId; - loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .8), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("r2d2.urdf", btVector3(-2, -4, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - - // Load one motor gripper for kuka - loadSdf("gripper/wsg50_one_motor_gripper_new_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true); - m_data->m_gripperId = bodyId + 1; - InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId); - InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId); - - // Reset the default gripper motor maximum torque for damping to 0 - for (int i = 0; i < gripperBody->m_multiBody->getNumLinks(); i++) - { - if (supportsJointMotor(gripperBody->m_multiBody, i)) - { - btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)gripperBody->m_multiBody->getLink(i).m_userPtr; - if (motor) - { - motor->setMaxAppliedImpulse(0); - } - } - } - - for (int i = 0; i < 6; i++) - { - loadUrdf("jenga/jenga.urdf", btVector3(1.3-0.1*i,-0.7, .75), btQuaternion(btVector3(0,1,0),SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - } - - //loadUrdf("nao/nao.urdf", btVector3(2,5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - - // Add slider joint for fingers - btVector3 pivotInParent1(-0.055, 0, 0.02); - btVector3 pivotInChild1(0, 0, 0); - btMatrix3x3 frameInParent1(btQuaternion(0, 0, 0, 1.0)); - btMatrix3x3 frameInChild1(btQuaternion(0, 0, 0, 1.0)); - btVector3 jointAxis1(1.0, 0, 0); - btVector3 pivotInParent2(0.055, 0, 0.02); - btVector3 pivotInChild2(0, 0, 0); - btMatrix3x3 frameInParent2(btQuaternion(0, 0, 0, 1.0)); - btMatrix3x3 frameInChild2(btQuaternion(0, 0, 1.0, 0)); - btVector3 jointAxis2(1.0, 0, 0); - m_data->m_kukaGripperRevolute1 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 2, gripperBody->m_multiBody, 4, pivotInParent1, pivotInChild1); - m_data->m_kukaGripperRevolute1->setMaxAppliedImpulse(5.0); - m_data->m_kukaGripperRevolute2 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 3, gripperBody->m_multiBody, 6, pivotInParent2, pivotInChild2); - m_data->m_kukaGripperRevolute2->setMaxAppliedImpulse(5.0); - - m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute1); - m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute2); - - if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs()==7) - { - gripperBody->m_multiBody->setHasSelfCollision(0); - btVector3 pivotInParent(0, 0, 0.05); - btMatrix3x3 frameInParent; - frameInParent.setIdentity(); - btVector3 pivotInChild(0, 0, 0); - btMatrix3x3 frameInChild; - frameInChild.setIdentity(); - - m_data->m_kukaGripperFixed = new btMultiBodyFixedConstraint(kukaBody->m_multiBody, 6, gripperBody->m_multiBody, 0, pivotInParent, pivotInChild, frameInParent, frameInChild); - m_data->m_kukaGripperMultiBody = gripperBody->m_multiBody; - m_data->m_kukaGripperFixed->setMaxAppliedImpulse(500); - m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperFixed); - } - - for (int i = 0; i < 10; i++) - { - loadUrdf("cube.urdf", btVector3(-4, -2, 0.5 + i), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - } - loadUrdf("sphere2.urdf", btVector3(-5, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("sphere2.urdf", btVector3(-5, 0, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("sphere2.urdf", btVector3(-5, 0, 3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - - btTransform objectLocalTr[] = { - btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.0, 0.0, 0.0)), - btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.0, 0.15, 0.64)), - btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.1, 0.15, 0.85)), - btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.4, 0.05, 0.85)), - btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.3, -0.05, 0.7)), - btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.1, 0.05, 0.7)), - btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.2, 0.15, 0.7)), - btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.2, 0.15, 0.9)), - btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.2, 0.05, 0.8)) - }; - - - btAlignedObjectArray objectWorldTr; - int numOb = sizeof(objectLocalTr) / sizeof(btTransform); - objectWorldTr.resize(numOb); - - btTransform tr; - tr.setIdentity(); - tr.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)); - tr.setOrigin(btVector3(1.0, -0.2, 0)); - - for (int i = 0; i < numOb; i++) - { - objectWorldTr[i] = tr*objectLocalTr[i]; - } - - // Table area - loadUrdf("table/table.urdf", objectWorldTr[0].getOrigin(), objectWorldTr[0].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("tray.urdf", objectWorldTr[1].getOrigin(), objectWorldTr[1].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - //loadUrdf("cup_small.urdf", objectWorldTr[2].getOrigin(), objectWorldTr[2].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - //loadUrdf("pitcher_small.urdf", objectWorldTr[3].getOrigin(), objectWorldTr[3].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("teddy_vhacd.urdf", objectWorldTr[4].getOrigin(), objectWorldTr[4].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("cube_small.urdf", objectWorldTr[5].getOrigin(), objectWorldTr[5].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("sphere_small.urdf", objectWorldTr[6].getOrigin(), objectWorldTr[6].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("duck_vhacd.urdf", objectWorldTr[7].getOrigin(), objectWorldTr[7].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - //loadUrdf("Apple/apple.urdf", objectWorldTr[8].getOrigin(), objectWorldTr[8].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - - // Shelf area - loadSdf("kiva_shelf/model.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true); - loadUrdf("teddy_vhacd.urdf", btVector3(-0.1, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("sphere_small.urdf", btVector3(-0.1, 0.6, 1.25), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("cube_small.urdf", btVector3(0.3, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - - // Chess area - loadUrdf("table_square/table_square.urdf", btVector3(-1.0, 0, 0.0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - //loadUrdf("pawn.urdf", btVector3(-0.8, -0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - //loadUrdf("queen.urdf", btVector3(-0.9, -0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - //loadUrdf("king.urdf", btVector3(-1.0, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - //loadUrdf("bishop.urdf", btVector3(-1.1, 0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - //loadUrdf("rook.urdf", btVector3(-1.2, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - //loadUrdf("knight.urdf", btVector3(-1.2, 0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - - //loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - m_data->m_huskyId = bodyId; - - m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10)); - - } - - if (m_data->m_kukaGripperFixed && m_data->m_kukaGripperMultiBody) - { - InteralBodyData* childBody = m_data->getHandle(m_data->m_gripperId); - // Add gripper controller - btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)childBody->m_multiBody->getLink(1).m_userPtr; - if (motor) - { - btScalar posTarget = (-0.048)*btMin(btScalar(0.75), gVRGripper2Analog) / 0.75; - motor->setPositionTarget(posTarget, .2); - motor->setVelocityTarget(0.0, .5); - motor->setMaxAppliedImpulse(5.0); - } - } - - if (m_data->m_gripperRigidbodyFixed && m_data->m_gripperMultiBody) - { - m_data->m_gripperRigidbodyFixed->setFrameInB(btMatrix3x3(gVRGripperOrn)); - m_data->m_gripperRigidbodyFixed->setPivotInB(gVRGripperPos); - for (int i = 0; i < m_data->m_gripperMultiBody->getNumLinks(); i++) - { - if (supportsJointMotor(m_data->m_gripperMultiBody, i)) - { - btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i ).m_userPtr; - if (motor) - { - motor->setErp(0.2); - btScalar posTarget = 0.1 + (1 - btMin(btScalar(0.75),gVRGripperAnalog)*btScalar(1.5))*SIMD_HALF_PI*0.29; - btScalar maxPosTarget = 0.55; - - if (m_data->m_gripperMultiBody->getJointPos(i) < 0) - { - m_data->m_gripperMultiBody->setJointPos(i,0); - } - if (m_data->m_gripperMultiBody->getJointPos(i) > maxPosTarget) - { - m_data->m_gripperMultiBody->setJointPos(i, maxPosTarget); - } - - motor->setPositionTarget(posTarget, 1); - motor->setVelocityTarget(0, 0.5); - btScalar maxImp = 1*m_data->m_physicsDeltaTime; - motor->setMaxAppliedImpulse(maxImp); - //motor->setRhsClamp(gRhsClamp); - } - } - } - } - - // Inverse kinematics for KUKA - //if (0) - { - InternalBodyHandle* bodyHandle = m_data->getHandle(m_data->m_KukaId); - if (bodyHandle && bodyHandle->m_multiBody && bodyHandle->m_multiBody->getNumDofs()==7) - { - btMultiBody* mb = bodyHandle->m_multiBody; - btScalar sqLen = (mb->getBaseWorldTransform().getOrigin() - gVRController2Pos).length2(); - btScalar distanceThreshold = 1.3; - gCloseToKuka=(sqLen<(distanceThreshold*distanceThreshold)); - - int numDofs = bodyHandle->m_multiBody->getNumDofs(); - btAlignedObjectArray q_new; - btAlignedObjectArray q_current; - q_current.resize(numDofs); - for (int i = 0; i < numDofs; i++) - { - q_current[i] = bodyHandle->m_multiBody->getJointPos(i); - } - - q_new.resize(numDofs); - //sensible rest-pose - q_new[0] = 0;// -SIMD_HALF_PI; - q_new[1] = 0; - q_new[2] = 0; - q_new[3] = SIMD_HALF_PI; - q_new[4] = 0; - q_new[5] = -SIMD_HALF_PI*0.66; - q_new[6] = 0; - - if (gCloseToKuka) - { - double dampIk[6] = {1.0, 1.0, 1.0, 1.0, 1.0, 0.0}; - - IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody); - IKTrajectoryHelper* ikHelperPtr = 0; - - if (ikHelperPtrPtr) - { - ikHelperPtr = *ikHelperPtrPtr; - } - else - { - IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper; - m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper); - ikHelperPtr = tmpHelper; - } - - int endEffectorLinkIndex = 6; - - if (ikHelperPtr && (endEffectorLinkIndexm_multiBody->getNumLinks())) - { - b3AlignedObjectArray jacobian_linear; - jacobian_linear.resize(3*numDofs); - b3AlignedObjectArray jacobian_angular; - jacobian_angular.resize(3*numDofs); - int jacSize = 0; - - btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); - - if (tree) - { - jacSize = jacobian_linear.size(); - // Set jacobian value - int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; - - btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs); - for (int i = 0; i < numDofs; i++) - { - q_current[i] = bodyHandle->m_multiBody->getJointPos(i); - q[i+baseDofs] = bodyHandle->m_multiBody->getJointPos(i); - qdot[i + baseDofs] = 0; - nu[i+baseDofs] = 0; - } - // Set the gravity to correspond to the world gravity - btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); - - if (-1 != tree->setGravityInWorldFrame(id_grav) && - -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) - { - tree->calculateJacobians(q); - btInverseDynamics::mat3x jac_t(3,numDofs); - btInverseDynamics::mat3x jac_r(3,numDofs); - tree->getBodyJacobianTrans(endEffectorLinkIndex, &jac_t); - tree->getBodyJacobianRot(endEffectorLinkIndex, &jac_r); - for (int i = 0; i < 3; ++i) - { - for (int j = 0; j < numDofs; ++j) - { - jacobian_linear[i*numDofs+j] = jac_t(i,j); - jacobian_angular[i*numDofs+j] = jac_r(i,j); - } - } - } - } - - int ikMethod= IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE;//IK2_VEL_DLS_WITH_ORIENTATION; //IK2_VEL_DLS; - - btVector3DoubleData endEffectorWorldPosition; - btVector3DoubleData endEffectorWorldOrientation; - btVector3DoubleData targetWorldPosition; - btVector3DoubleData targetWorldOrientation; - - btVector3 endEffectorPosWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getOrigin(); - btQuaternion endEffectorOriWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getRotation(); - btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w()); - - // Prescribed position and orientation - static btScalar time=0.f; - time+=0.01; - btVector3 targetPos(0.4-0.4*b3Cos( time), 0, 0.8+0.4*b3Cos( time)); - targetPos +=mb->getBasePos(); - btVector4 downOrn(0,1,0,0); - - // Controller orientation - btVector4 controllerOrn(gVRController2Orn.x(), gVRController2Orn.y(), gVRController2Orn.z(), gVRController2Orn.w()); - - // Set position and orientation - endEffectorPosWorld.serializeDouble(endEffectorWorldPosition); - endEffectorOri.serializeDouble(endEffectorWorldOrientation); - downOrn.serializeDouble(targetWorldOrientation); - //targetPos.serializeDouble(targetWorldPosition); - - gVRController2Pos.serializeDouble(targetWorldPosition); - - //controllerOrn.serializeDouble(targetWorldOrientation); - - if (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE) - { - btAlignedObjectArray lower_limit; - btAlignedObjectArray upper_limit; - btAlignedObjectArray joint_range; - btAlignedObjectArray rest_pose; - lower_limit.resize(numDofs); - upper_limit.resize(numDofs); - joint_range.resize(numDofs); - rest_pose.resize(numDofs); - lower_limit[0] = -.967; - lower_limit[1] = -2.0; - lower_limit[2] = -2.96; - lower_limit[3] = 0.19; - lower_limit[4] = -2.96; - lower_limit[5] = -2.09; - lower_limit[6] = -3.05; - upper_limit[0] = .96; - upper_limit[1] = 2.0; - upper_limit[2] = 2.96; - upper_limit[3] = 2.29; - upper_limit[4] = 2.96; - upper_limit[5] = 2.09; - upper_limit[6] = 3.05; - joint_range[0] = 5.8; - joint_range[1] = 4; - joint_range[2] = 5.8; - joint_range[3] = 4; - joint_range[4] = 5.8; - joint_range[5] = 4; - joint_range[6] = 6; - rest_pose[0] = 0; - rest_pose[1] = 0; - rest_pose[2] = 0; - rest_pose[3] = SIMD_HALF_PI; - rest_pose[4] = 0; - rest_pose[5] = -SIMD_HALF_PI*0.66; - rest_pose[6] = 0; - ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); - } - - ikHelperPtr->computeIK(targetWorldPosition.m_floats, targetWorldOrientation.m_floats, - endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, - &q_current[0], - numDofs, endEffectorLinkIndex, - &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, dampIk); - } - } - - //directly set the position of the links, only for debugging IK, don't use this method! - if (0) - { - for (int i=0;igetNumLinks();i++) - { - btScalar desiredPosition = q_new[i]; - mb->setJointPosMultiDof(i,&desiredPosition); - } - } else - { - int numMotors = 0; - //find the joint motors and apply the desired velocity and maximum force/torque - { - int velIndex = 6;//skip the 3 linear + 3 angular degree of freedom velocity entries of the base - int posIndex = 7;//skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base - for (int link=0;linkgetNumLinks();link++) - { - if (supportsJointMotor(mb,link)) - { - btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr; - - if (motor) - { - btScalar desiredVelocity = 0.f; - btScalar desiredPosition = q_new[link]; - motor->setRhsClamp(gRhsClamp); - //printf("link %d: %f", link, q_new[link]); - motor->setVelocityTarget(desiredVelocity,1.0); - motor->setPositionTarget(desiredPosition,0.6); - btScalar maxImp = 1.0; - - motor->setMaxAppliedImpulse(maxImp); - numMotors++; - } - } - velIndex += mb->getLink(link).m_dofCount; - posIndex += mb->getLink(link).m_posVarCount; - } - } - } - } - - } + createDefaultRobotAssets(); } int maxSteps = m_data->m_numSimulationSubSteps+3; @@ -3732,3 +3279,472 @@ void PhysicsServerCommandProcessor::applyJointDamping(int bodyUniqueId) } } + +//todo: move this to Python/scripting +void PhysicsServerCommandProcessor::createDefaultRobotAssets() +{ + static btAlignedObjectArray gBufferServerToClient; + gBufferServerToClient.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + int bodyId = 0; + + if (gCreateObjectSimVR >= 0) + { + gCreateObjectSimVR = -1; + btMatrix3x3 mat(gVRGripperOrn); + btScalar spawnDistance = 0.1; + btVector3 spawnDir = mat.getColumn(0); + btVector3 shiftPos = spawnDir*spawnDistance; + btVector3 spawnPos = gVRGripperPos + shiftPos; + loadUrdf("sphere_small.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + //loadUrdf("lego/lego.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + m_data->m_sphereId = bodyId; + InteralBodyData* parentBody = m_data->getHandle(bodyId); + if (parentBody->m_multiBody) + { + parentBody->m_multiBody->setBaseVel(spawnDir * 5); + } + } + + if (!m_data->m_hasGround) + { + m_data->m_hasGround = true; + + loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("samurai.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + + if (m_data->m_gripperRigidbodyFixed == 0) + { + int bodyId = 0; + + if (loadUrdf("pr2_gripper.urdf", btVector3(0, 0, 0.1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size())) + { + InteralBodyData* parentBody = m_data->getHandle(bodyId); + if (parentBody->m_multiBody) + { + parentBody->m_multiBody->setHasSelfCollision(0); + btVector3 pivotInParent(0.2, 0, 0); + btMatrix3x3 frameInParent; + //frameInParent.setRotation(btQuaternion(0, 0, 0, 1)); + frameInParent.setIdentity(); + btVector3 pivotInChild(0, 0, 0); + btMatrix3x3 frameInChild; + frameInChild.setIdentity(); + + m_data->m_gripperRigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody, -1, 0, pivotInParent, pivotInChild, frameInParent, frameInChild); + m_data->m_gripperMultiBody = parentBody->m_multiBody; + if (m_data->m_gripperMultiBody->getNumLinks() > 2) + { + m_data->m_gripperMultiBody->setJointPos(0, 0); + m_data->m_gripperMultiBody->setJointPos(2, 0); + } + m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(500); + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld; + world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed); + } + } + } + + loadUrdf("kuka_iiwa/model_vr_limits.urdf", btVector3(1.4, -0.2, 0.6), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + m_data->m_KukaId = bodyId; + loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .8), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("r2d2.urdf", btVector3(-2, -4, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + + // Load one motor gripper for kuka + loadSdf("gripper/wsg50_one_motor_gripper_new_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true); + m_data->m_gripperId = bodyId + 1; + InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId); + InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId); + + // Reset the default gripper motor maximum torque for damping to 0 + for (int i = 0; i < gripperBody->m_multiBody->getNumLinks(); i++) + { + if (supportsJointMotor(gripperBody->m_multiBody, i)) + { + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)gripperBody->m_multiBody->getLink(i).m_userPtr; + if (motor) + { + motor->setMaxAppliedImpulse(0); + } + } + } + + for (int i = 0; i < 6; i++) + { + loadUrdf("jenga/jenga.urdf", btVector3(1.3-0.1*i,-0.7, .75), btQuaternion(btVector3(0,1,0),SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + } + + //loadUrdf("nao/nao.urdf", btVector3(2,5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + + // Add slider joint for fingers + btVector3 pivotInParent1(-0.055, 0, 0.02); + btVector3 pivotInChild1(0, 0, 0); + btMatrix3x3 frameInParent1(btQuaternion(0, 0, 0, 1.0)); + btMatrix3x3 frameInChild1(btQuaternion(0, 0, 0, 1.0)); + btVector3 jointAxis1(1.0, 0, 0); + btVector3 pivotInParent2(0.055, 0, 0.02); + btVector3 pivotInChild2(0, 0, 0); + btMatrix3x3 frameInParent2(btQuaternion(0, 0, 0, 1.0)); + btMatrix3x3 frameInChild2(btQuaternion(0, 0, 1.0, 0)); + btVector3 jointAxis2(1.0, 0, 0); + m_data->m_kukaGripperRevolute1 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 2, gripperBody->m_multiBody, 4, pivotInParent1, pivotInChild1); + m_data->m_kukaGripperRevolute1->setMaxAppliedImpulse(5.0); + m_data->m_kukaGripperRevolute2 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 3, gripperBody->m_multiBody, 6, pivotInParent2, pivotInChild2); + m_data->m_kukaGripperRevolute2->setMaxAppliedImpulse(5.0); + + m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute1); + m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute2); + + if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs()==7) + { + gripperBody->m_multiBody->setHasSelfCollision(0); + btVector3 pivotInParent(0, 0, 0.05); + btMatrix3x3 frameInParent; + frameInParent.setIdentity(); + btVector3 pivotInChild(0, 0, 0); + btMatrix3x3 frameInChild; + frameInChild.setIdentity(); + + m_data->m_kukaGripperFixed = new btMultiBodyFixedConstraint(kukaBody->m_multiBody, 6, gripperBody->m_multiBody, 0, pivotInParent, pivotInChild, frameInParent, frameInChild); + m_data->m_kukaGripperMultiBody = gripperBody->m_multiBody; + m_data->m_kukaGripperFixed->setMaxAppliedImpulse(500); + m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperFixed); + } + + for (int i = 0; i < 10; i++) + { + loadUrdf("cube.urdf", btVector3(-4, -2, 0.5 + i), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + } + loadUrdf("sphere2.urdf", btVector3(-5, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("sphere2.urdf", btVector3(-5, 0, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("sphere2.urdf", btVector3(-5, 0, 3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + + btTransform objectLocalTr[] = { + btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.0, 0.0, 0.0)), + btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.0, 0.15, 0.64)), + btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.1, 0.15, 0.85)), + btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.4, 0.05, 0.85)), + btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.3, -0.05, 0.7)), + btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.1, 0.05, 0.7)), + btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.2, 0.15, 0.7)), + btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.2, 0.15, 0.9)), + btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.2, 0.05, 0.8)) + }; + + + btAlignedObjectArray objectWorldTr; + int numOb = sizeof(objectLocalTr) / sizeof(btTransform); + objectWorldTr.resize(numOb); + + btTransform tr; + tr.setIdentity(); + tr.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)); + tr.setOrigin(btVector3(1.0, -0.2, 0)); + + for (int i = 0; i < numOb; i++) + { + objectWorldTr[i] = tr*objectLocalTr[i]; + } + + // Table area + loadUrdf("table/table.urdf", objectWorldTr[0].getOrigin(), objectWorldTr[0].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("tray.urdf", objectWorldTr[1].getOrigin(), objectWorldTr[1].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + //loadUrdf("cup_small.urdf", objectWorldTr[2].getOrigin(), objectWorldTr[2].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + //loadUrdf("pitcher_small.urdf", objectWorldTr[3].getOrigin(), objectWorldTr[3].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("teddy_vhacd.urdf", objectWorldTr[4].getOrigin(), objectWorldTr[4].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("cube_small.urdf", objectWorldTr[5].getOrigin(), objectWorldTr[5].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("sphere_small.urdf", objectWorldTr[6].getOrigin(), objectWorldTr[6].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("duck_vhacd.urdf", objectWorldTr[7].getOrigin(), objectWorldTr[7].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + //loadUrdf("Apple/apple.urdf", objectWorldTr[8].getOrigin(), objectWorldTr[8].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + + // Shelf area + loadSdf("kiva_shelf/model.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true); + loadUrdf("teddy_vhacd.urdf", btVector3(-0.1, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("sphere_small.urdf", btVector3(-0.1, 0.6, 1.25), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("cube_small.urdf", btVector3(0.3, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + + // Chess area + loadUrdf("table_square/table_square.urdf", btVector3(-1.0, 0, 0.0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + //loadUrdf("pawn.urdf", btVector3(-0.8, -0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + //loadUrdf("queen.urdf", btVector3(-0.9, -0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + //loadUrdf("king.urdf", btVector3(-1.0, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + //loadUrdf("bishop.urdf", btVector3(-1.1, 0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + //loadUrdf("rook.urdf", btVector3(-1.2, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + //loadUrdf("knight.urdf", btVector3(-1.2, 0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + + //loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + m_data->m_huskyId = bodyId; + + m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10)); + + } + + if (m_data->m_kukaGripperFixed && m_data->m_kukaGripperMultiBody) + { + InteralBodyData* childBody = m_data->getHandle(m_data->m_gripperId); + // Add gripper controller + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)childBody->m_multiBody->getLink(1).m_userPtr; + if (motor) + { + btScalar posTarget = (-0.048)*btMin(btScalar(0.75), gVRGripper2Analog) / 0.75; + motor->setPositionTarget(posTarget, .2); + motor->setVelocityTarget(0.0, .5); + motor->setMaxAppliedImpulse(5.0); + } + } + + if (m_data->m_gripperRigidbodyFixed && m_data->m_gripperMultiBody) + { + m_data->m_gripperRigidbodyFixed->setFrameInB(btMatrix3x3(gVRGripperOrn)); + m_data->m_gripperRigidbodyFixed->setPivotInB(gVRGripperPos); + for (int i = 0; i < m_data->m_gripperMultiBody->getNumLinks(); i++) + { + if (supportsJointMotor(m_data->m_gripperMultiBody, i)) + { + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i ).m_userPtr; + if (motor) + { + motor->setErp(0.2); + btScalar posTarget = 0.1 + (1 - btMin(btScalar(0.75),gVRGripperAnalog)*btScalar(1.5))*SIMD_HALF_PI*0.29; + btScalar maxPosTarget = 0.55; + + if (m_data->m_gripperMultiBody->getJointPos(i) < 0) + { + m_data->m_gripperMultiBody->setJointPos(i,0); + } + if (m_data->m_gripperMultiBody->getJointPos(i) > maxPosTarget) + { + m_data->m_gripperMultiBody->setJointPos(i, maxPosTarget); + } + + motor->setPositionTarget(posTarget, 1); + motor->setVelocityTarget(0, 0.5); + btScalar maxImp = 1*m_data->m_physicsDeltaTime; + motor->setMaxAppliedImpulse(maxImp); + //motor->setRhsClamp(gRhsClamp); + } + } + } + } + + // Inverse kinematics for KUKA + //if (0) + { + InternalBodyHandle* bodyHandle = m_data->getHandle(m_data->m_KukaId); + if (bodyHandle && bodyHandle->m_multiBody && bodyHandle->m_multiBody->getNumDofs()==7) + { + btMultiBody* mb = bodyHandle->m_multiBody; + btScalar sqLen = (mb->getBaseWorldTransform().getOrigin() - gVRController2Pos).length2(); + btScalar distanceThreshold = 1.3; + gCloseToKuka=(sqLen<(distanceThreshold*distanceThreshold)); + + int numDofs = bodyHandle->m_multiBody->getNumDofs(); + btAlignedObjectArray q_new; + btAlignedObjectArray q_current; + q_current.resize(numDofs); + for (int i = 0; i < numDofs; i++) + { + q_current[i] = bodyHandle->m_multiBody->getJointPos(i); + } + + q_new.resize(numDofs); + //sensible rest-pose + q_new[0] = 0;// -SIMD_HALF_PI; + q_new[1] = 0; + q_new[2] = 0; + q_new[3] = SIMD_HALF_PI; + q_new[4] = 0; + q_new[5] = -SIMD_HALF_PI*0.66; + q_new[6] = 0; + + if (gCloseToKuka) + { + double dampIk[6] = {1.0, 1.0, 1.0, 1.0, 1.0, 0.0}; + + IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody); + IKTrajectoryHelper* ikHelperPtr = 0; + + if (ikHelperPtrPtr) + { + ikHelperPtr = *ikHelperPtrPtr; + } + else + { + IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper; + m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper); + ikHelperPtr = tmpHelper; + } + + int endEffectorLinkIndex = 6; + + if (ikHelperPtr && (endEffectorLinkIndexm_multiBody->getNumLinks())) + { + b3AlignedObjectArray jacobian_linear; + jacobian_linear.resize(3*numDofs); + b3AlignedObjectArray jacobian_angular; + jacobian_angular.resize(3*numDofs); + int jacSize = 0; + + btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); + + if (tree) + { + jacSize = jacobian_linear.size(); + // Set jacobian value + int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; + + btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs); + for (int i = 0; i < numDofs; i++) + { + q_current[i] = bodyHandle->m_multiBody->getJointPos(i); + q[i+baseDofs] = bodyHandle->m_multiBody->getJointPos(i); + qdot[i + baseDofs] = 0; + nu[i+baseDofs] = 0; + } + // Set the gravity to correspond to the world gravity + btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); + + if (-1 != tree->setGravityInWorldFrame(id_grav) && + -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) + { + tree->calculateJacobians(q); + btInverseDynamics::mat3x jac_t(3,numDofs); + btInverseDynamics::mat3x jac_r(3,numDofs); + tree->getBodyJacobianTrans(endEffectorLinkIndex, &jac_t); + tree->getBodyJacobianRot(endEffectorLinkIndex, &jac_r); + for (int i = 0; i < 3; ++i) + { + for (int j = 0; j < numDofs; ++j) + { + jacobian_linear[i*numDofs+j] = jac_t(i,j); + jacobian_angular[i*numDofs+j] = jac_r(i,j); + } + } + } + } + + int ikMethod= IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE;//IK2_VEL_DLS_WITH_ORIENTATION; //IK2_VEL_DLS; + + btVector3DoubleData endEffectorWorldPosition; + btVector3DoubleData endEffectorWorldOrientation; + btVector3DoubleData targetWorldPosition; + btVector3DoubleData targetWorldOrientation; + + btVector3 endEffectorPosWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getOrigin(); + btQuaternion endEffectorOriWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getRotation(); + btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w()); + + // Prescribed position and orientation + static btScalar time=0.f; + time+=0.01; + btVector3 targetPos(0.4-0.4*b3Cos( time), 0, 0.8+0.4*b3Cos( time)); + targetPos +=mb->getBasePos(); + btVector4 downOrn(0,1,0,0); + + // Controller orientation + btVector4 controllerOrn(gVRController2Orn.x(), gVRController2Orn.y(), gVRController2Orn.z(), gVRController2Orn.w()); + + // Set position and orientation + endEffectorPosWorld.serializeDouble(endEffectorWorldPosition); + endEffectorOri.serializeDouble(endEffectorWorldOrientation); + downOrn.serializeDouble(targetWorldOrientation); + //targetPos.serializeDouble(targetWorldPosition); + + gVRController2Pos.serializeDouble(targetWorldPosition); + + //controllerOrn.serializeDouble(targetWorldOrientation); + + if (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE) + { + btAlignedObjectArray lower_limit; + btAlignedObjectArray upper_limit; + btAlignedObjectArray joint_range; + btAlignedObjectArray rest_pose; + lower_limit.resize(numDofs); + upper_limit.resize(numDofs); + joint_range.resize(numDofs); + rest_pose.resize(numDofs); + lower_limit[0] = -.967; + lower_limit[1] = -2.0; + lower_limit[2] = -2.96; + lower_limit[3] = 0.19; + lower_limit[4] = -2.96; + lower_limit[5] = -2.09; + lower_limit[6] = -3.05; + upper_limit[0] = .96; + upper_limit[1] = 2.0; + upper_limit[2] = 2.96; + upper_limit[3] = 2.29; + upper_limit[4] = 2.96; + upper_limit[5] = 2.09; + upper_limit[6] = 3.05; + joint_range[0] = 5.8; + joint_range[1] = 4; + joint_range[2] = 5.8; + joint_range[3] = 4; + joint_range[4] = 5.8; + joint_range[5] = 4; + joint_range[6] = 6; + rest_pose[0] = 0; + rest_pose[1] = 0; + rest_pose[2] = 0; + rest_pose[3] = SIMD_HALF_PI; + rest_pose[4] = 0; + rest_pose[5] = -SIMD_HALF_PI*0.66; + rest_pose[6] = 0; + ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); + } + + ikHelperPtr->computeIK(targetWorldPosition.m_floats, targetWorldOrientation.m_floats, + endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, + &q_current[0], + numDofs, endEffectorLinkIndex, + &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, dampIk); + } + } + + //directly set the position of the links, only for debugging IK, don't use this method! + if (0) + { + for (int i=0;igetNumLinks();i++) + { + btScalar desiredPosition = q_new[i]; + mb->setJointPosMultiDof(i,&desiredPosition); + } + } else + { + int numMotors = 0; + //find the joint motors and apply the desired velocity and maximum force/torque + { + int velIndex = 6;//skip the 3 linear + 3 angular degree of freedom velocity entries of the base + int posIndex = 7;//skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base + for (int link=0;linkgetNumLinks();link++) + { + if (supportsJointMotor(mb,link)) + { + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr; + + if (motor) + { + btScalar desiredVelocity = 0.f; + btScalar desiredPosition = q_new[link]; + motor->setRhsClamp(gRhsClamp); + //printf("link %d: %f", link, q_new[link]); + motor->setVelocityTarget(desiredVelocity,1.0); + motor->setPositionTarget(desiredPosition,0.6); + btScalar maxImp = 1.0; + + motor->setMaxAppliedImpulse(maxImp); + numMotors++; + } + } + velIndex += mb->getLink(link).m_dofCount; + posIndex += mb->getLink(link).m_posVarCount; + } + } + } + } + + } +} \ No newline at end of file diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 6526fe030..2814d11dd 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -16,6 +16,9 @@ class PhysicsServerCommandProcessor struct PhysicsServerCommandProcessorInternalData* m_data; + //todo: move this to physics client side / Python + void createDefaultRobotAssets(); + protected: diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index ef7f1404f..77eaa6cd9 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -34,13 +34,15 @@ extern btScalar gVRGripperAnalog; extern btScalar gVRGripper2Analog; extern bool gCloseToKuka; extern bool gEnableRealTimeSimVR; -extern bool gCreateSamuraiRobotAssets; +extern bool gCreateDefaultRobotAssets; +extern int gInternalSimFlags; extern int gCreateObjectSimVR; static int gGraspingController = -1; extern btScalar simTimeScalingFactor; extern bool gVRGripperClosed; + bool gDebugRenderToggle = false; void MotionThreadFunc(void* userPtr,void* lsMemory); void* MotionlsMemoryFunc(); @@ -599,6 +601,7 @@ public: virtual void vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orientation[4]); virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4], float analogAxis); + virtual bool mouseMoveCallback(float x,float y) { @@ -670,13 +673,18 @@ public: virtual void processCommandLineArgs(int argc, char* argv[]) { b3CommandLineArgs args(argc,argv); - if (args.CheckCmdLineFlag("emptyworld")) + if (args.CheckCmdLineFlag("robotassets")) { - gCreateSamuraiRobotAssets = false; + gCreateDefaultRobotAssets = true; } - } - + if (args.CheckCmdLineFlag("norobotassets")) + { + gCreateDefaultRobotAssets = false; + } + + + } }; @@ -995,7 +1003,7 @@ void PhysicsServerExample::renderScene() } #ifdef BT_ENABLE_VR - if (m_tinyVrGui==0) + if ((gInternalSimFlags&2 ) && m_tinyVrGui==0) { ComboBoxParams comboParams; comboParams.m_comboboxId = 0; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 5a1141821..0b7f325f8 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -238,9 +238,16 @@ enum EnumSimParamUpdateFlags SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS=4, SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS=8, SIM_PARAM_UPDATE_REAL_TIME_SIMULATION = 16, - SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP=32 + SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP=32, + SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS=64 }; +enum EnumSimParamInternalSimFlags +{ + SIM_PARAM_INTERNAL_CREATE_ROBOT_ASSETS=1, +}; + + ///Controlling a robot involves sending the desired state to its joint motor controllers. ///The control mode determines the state variables used for motor control. struct SendPhysicsSimulationParameters @@ -250,6 +257,7 @@ struct SendPhysicsSimulationParameters int m_numSimulationSubSteps; int m_numSolverIterations; bool m_allowRealTimeSimulation; + int m_internalSimFlags; double m_defaultContactERP; }; diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index 448ac094a..89b8cfe29 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -2199,7 +2199,15 @@ int main(int argc, char *argv[]) if (sExample) { + //until we have a proper VR gui, always assume we want the hard-coded default robot assets + char* newargv[2]; + char* t0 = (char*)"--robotassets"; + newargv[0] = t0; + newargv[1] = t0; + sExample->processCommandLineArgs(2,newargv); + sExample->processCommandLineArgs(argc,argv); + } //request disable VSYNC diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 1af256b47..744340f1e 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -497,6 +497,39 @@ static PyObject* pybullet_setRealTimeSimulation(PyObject* self, return Py_None; } + + +static PyObject* pybullet_setInternalSimFlags(PyObject* self, + PyObject* args) { + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + int enableRealTimeSimulation = 0; + int ret; + + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; + + if (!PyArg_ParseTuple(args, "i", &enableRealTimeSimulation)) { + PyErr_SetString( + SpamError, + "setInternalSimFlags expected a single value (integer)."); + return NULL; + } + ret = + b3PhysicsParamSetInternalSimFlags(command, enableRealTimeSimulation); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); + } + + Py_INCREF(Py_None); + return Py_None; +} + // Set the gravity of the world with (x, y, z) arguments static PyObject* pybullet_setGravity(PyObject* self, PyObject* args) { if (0 == sm) { @@ -2218,6 +2251,9 @@ static PyMethodDef SpamMethods[] = { "Enable or disable real time simulation (using the real time clock," " RTC) in the physics server. Expects one integer argument, 0 or 1" }, + { "setInternalSimFlags", pybullet_setInternalSimFlags, METH_VARARGS, + "This is for experimental purposes, use at own risk, magic may or not happen"}, + {"loadURDF", pybullet_loadURDF, METH_VARARGS, "Create a multibody by loading a URDF file."},