From e81845249428b447ef50638c92a20a5c561b7dcb Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Tue, 5 Mar 2019 15:44:06 -0800 Subject: [PATCH] Add grasp box example and fixjoint boxes example. tmp gripper is working? fix joint example --- examples/BulletRobotics/FixJointBoxes.cpp | 107 +++++++++--- examples/BulletRobotics/GraspBox.cpp | 186 +++++++++++++++++++++ examples/BulletRobotics/GraspBox.h | 21 +++ examples/BulletRobotics/JointLimit.cpp | 13 +- examples/ExampleBrowser/CMakeLists.txt | 2 + examples/ExampleBrowser/ExampleEntries.cpp | 5 + 6 files changed, 300 insertions(+), 34 deletions(-) create mode 100644 examples/BulletRobotics/GraspBox.cpp create mode 100644 examples/BulletRobotics/GraspBox.h diff --git a/examples/BulletRobotics/FixJointBoxes.cpp b/examples/BulletRobotics/FixJointBoxes.cpp index 875a76b1d..3acf80758 100644 --- a/examples/BulletRobotics/FixJointBoxes.cpp +++ b/examples/BulletRobotics/FixJointBoxes.cpp @@ -14,16 +14,26 @@ #include #include "../RobotSimulator/b3RobotSimulatorClientAPI.h" +static btScalar numSolverIterations = 1000; +static btScalar solverId = 0; + class FixJointBoxes : public CommonExampleInterface { GUIHelperInterface* m_guiHelper; b3RobotSimulatorClientAPI m_robotSim; int m_options; + b3RobotSimulatorSetPhysicsEngineParameters physicsArgs; + int solver; + + const size_t numCubes = 30; + std::vector cubeIds; public: FixJointBoxes(GUIHelperInterface* helper, int options) : m_guiHelper(helper), - m_options(options) + m_options(options), + cubeIds(numCubes, 0), + solver(solverId) { } @@ -47,39 +57,84 @@ public: m_robotSim.configureDebugVisualizer(COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0); m_robotSim.configureDebugVisualizer(COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0); - b3RobotSimulatorLoadUrdfFileArgs args; - - size_t numCubes = 10; - std::vector cubeIds(numCubes, 0); - for (int i = 0; i < numCubes; i++) { - args.m_forceOverrideFixedBase = (i == 0); - args.m_startPosition.setValue(0, i * 0.05, 1); - cubeIds[i] = m_robotSim.loadURDF("cube_small.urdf", args); + b3RobotSimulatorLoadUrdfFileArgs args; + b3RobotSimulatorChangeDynamicsArgs dynamicsArgs; - b3JointInfo jointInfo; - - jointInfo.m_parentFrame[1] = -0.025; - jointInfo.m_childFrame[1] = 0.025; - jointInfo.m_jointType = eFixedType; - // jointInfo.m_jointType = ePoint2PointType; - // jointInfo.m_jointType = ePrismaticType; - - if (i > 0) + for (int i = 0; i < numCubes; i++) { - m_robotSim.createConstraint(cubeIds[i], -1, cubeIds[i - 1], -1, &jointInfo); - } + args.m_forceOverrideFixedBase = (i == 0); + args.m_startPosition.setValue(0, i * 0.05, 1); + cubeIds[i] = m_robotSim.loadURDF("cube_small.urdf", args); - m_robotSim.loadURDF("plane.urdf"); + b3RobotJointInfo jointInfo; + + jointInfo.m_parentFrame[1] = -0.025; + jointInfo.m_childFrame[1] = 0.025; + + if (i > 0) + { + m_robotSim.createConstraint(cubeIds[i], -1, cubeIds[i - 1], -1, &jointInfo); + } + + m_robotSim.loadURDF("plane.urdf"); + } } + + { + SliderParams slider("Direct solver", &solverId); + slider.m_minVal = 0; + slider.m_maxVal = 1; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { + SliderParams slider("numSolverIterations", &numSolverIterations); + slider.m_minVal = 50; + slider.m_maxVal = 1e4; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + physicsArgs.m_defaultGlobalCFM = 1e-6; + m_robotSim.setPhysicsEngineParameter(physicsArgs); + + m_robotSim.setGravity(btVector3(0, 0, -10)); + m_robotSim.setNumSolverIterations((int)numSolverIterations); } virtual void exitPhysics() { m_robotSim.disconnect(); } + + void resetCubePosition() + { + for (int i = 0; i < numCubes; i++) + { + btVector3 pos = {0, i * 0.05, 1}; + btQuaternion quar = {0, 0, 0, 1}; + m_robotSim.resetBasePositionAndOrientation(cubeIds[i], pos, quar); + } + } virtual void stepSimulation(float deltaTime) { + int newSolver = (int)(solverId + 0.5); + if (newSolver != solver) + { + printf("Switching solver, new %d, old %d\n", newSolver, solver); + solver = newSolver; + resetCubePosition(); + if (solver) + { + physicsArgs.m_constraintSolverType = eConstraintSolverLCP_DANTZIG; + } + else + { + physicsArgs.m_constraintSolverType = eConstraintSolverLCP_SI; + } + + m_robotSim.setPhysicsEngineParameter(physicsArgs); + } + m_robotSim.setNumSolverIterations((int)numSolverIterations); m_robotSim.stepSimulation(); } virtual void renderScene() @@ -102,12 +157,12 @@ public: virtual void resetCamera() { - // float dist = 1; - // float pitch = -20; - // float yaw = -30; - // float targetPos[3] = {0, 0.2, 0.5}; + float dist = 1.2; + float pitch = -20; + float yaw = 90; + float targetPos[3] = {.5, 0.6, 0.5}; - // m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); } }; diff --git a/examples/BulletRobotics/GraspBox.cpp b/examples/BulletRobotics/GraspBox.cpp new file mode 100644 index 000000000..6a8d077fa --- /dev/null +++ b/examples/BulletRobotics/GraspBox.cpp @@ -0,0 +1,186 @@ + +#include "GraspBox.h" + +#include "../CommonInterfaces/CommonGraphicsAppInterface.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "../CommonInterfaces/CommonRenderInterface.h" +#include "../CommonInterfaces/CommonExampleInterface.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" +#include "../SharedMemory/PhysicsServerSharedMemory.h" +#include "../SharedMemory/PhysicsClientC_API.h" +#include "../CommonInterfaces/CommonParameterInterface.h" +#include "../SharedMemory/SharedMemoryPublic.h" +#include +#include "../RobotSimulator/b3RobotSimulatorClientAPI.h" + +static btScalar x = 0.401f; +static btScalar y = 0.025f; +static btScalar z = 0.44f; + +class GraspBox : public CommonExampleInterface +{ + GUIHelperInterface* m_guiHelper; + b3RobotSimulatorClientAPI m_robotSim; + int m_options; + + int m_armIndex; + int boxId; + +public: + GraspBox(GUIHelperInterface* helper, int options) + : m_guiHelper(helper), + m_options(options) + { + } + + virtual ~GraspBox() + { + } + + virtual void physicsDebugDraw(int debugDrawMode) + { + m_robotSim.debugDraw(debugDrawMode); + } + virtual void initPhysics() + { + int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER; + m_robotSim.setGuiHelper(m_guiHelper); + bool connected = m_robotSim.connect(mode); + + b3Printf("robotSim connected = %d", connected); + + m_robotSim.configureDebugVisualizer(COV_ENABLE_RGB_BUFFER_PREVIEW, 0); + m_robotSim.configureDebugVisualizer(COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0); + m_robotSim.configureDebugVisualizer(COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0); + + double cubePos[3] = {0.41, 0, 0.5}; + double cubeHalfLength = 0.025; + + { + SliderParams slider("position x", &x); + slider.m_minVal = 0; + slider.m_maxVal = 1; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + { + SliderParams slider("position y", &y); + slider.m_minVal = 0; + slider.m_maxVal = 1; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { + SliderParams slider("position z", &z); + slider.m_minVal = 0; + slider.m_maxVal = 1; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + { + b3RobotSimulatorLoadFileResults results; + m_robotSim.loadSDF("kuka_iiwa/kuka_with_gripper.sdf", results); + if (results.m_uniqueObjectIds.size() == 1) + { + m_armIndex = results.m_uniqueObjectIds[0]; + int numJoints = m_robotSim.getNumJoints(m_armIndex); + b3Printf("numJoints = %d", numJoints); + + for (int i = 0; i < numJoints; i++) + { + b3RobotJointInfo jointInfo; + m_robotSim.getJointInfo(m_armIndex, i, &jointInfo); + b3Printf("joint[%d].m_jointName=%s", i, jointInfo.m_jointName); + } + + //adjust arm position + m_robotSim.resetJointState(m_armIndex, 3, -SIMD_HALF_PI); + m_robotSim.resetJointState(m_armIndex, 5, SIMD_HALF_PI); + + // adjust gripper position + m_robotSim.resetJointState(m_armIndex, 8, -0.12); + m_robotSim.resetJointState(m_armIndex, 11, 0.12); + + m_robotSim.resetJointState(m_armIndex, 10, -0.12); + m_robotSim.resetJointState(m_armIndex, 13, 0.12); + + // { + // b3RobotJointInfo jointInfo; + // m_robotSim.getJointInfo(m_armIndex, 8, &jointInfo); + // jointInfo.m_jointType = eGearType; + // m_robotSim.createConstraint(m_armIndex, , cubeIds[i - 1], -1, &jointInfo); + // } + // for(int i = 0; i<14; i++){ + // b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + // controlArgs.m_maxTorqueValue = 0.0; + // m_robotSim.setJointMotorControl(m_armIndex, i, controlArgs); + // } + } + } + + { + b3RobotSimulatorLoadUrdfFileArgs args; + args.m_startPosition.setValue(x, y, z); + boxId = m_robotSim.loadURDF("cube_small.urdf", args); + } + m_robotSim.loadURDF("plane.urdf"); + m_robotSim.setGravity(btVector3(0, 0, -10)); + } + + virtual void exitPhysics() + { + m_robotSim.disconnect(); + } + + virtual void stepSimulation(float deltaTime) + { + btScalar sGripperVerticalVelocity = 0.f; + btScalar sGripperClosingTargetVelocity = -0.7f; + + int fingerJointIndices[4] = {8, 10, 11, 13}; + double fingerTargetVelocities[4] = {0.4, 0.4, -0.4, -0.4}; + + double maxTorqueValues[4] = {20.0, 20.0, 20.0, 20.0}; + for (int i = 0; i < 4; i++) + { + b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + controlArgs.m_targetVelocity = fingerTargetVelocities[i]; + controlArgs.m_maxTorqueValue = maxTorqueValues[i]; + controlArgs.m_kd = 1.; + m_robotSim.setJointMotorControl(m_armIndex, fingerJointIndices[i], controlArgs); + } + + m_robotSim.stepSimulation(); + } + virtual void renderScene() + { + m_robotSim.renderScene(); + } + + virtual bool mouseMoveCallback(float x, float y) + { + return m_robotSim.mouseMoveCallback(x, y); + } + virtual bool mouseButtonCallback(int button, int state, float x, float y) + { + return m_robotSim.mouseButtonCallback(button, state, x, y); + } + virtual bool keyboardCallback(int key, int state) + { + return false; + } + + virtual void resetCamera() + { + float dist = 0.8; + float pitch = 0; + float yaw = 0; + float targetPos[3] = {.4, 0.6, 0.45}; + + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } +}; + +class CommonExampleInterface* GraspBoxCreateFunc(struct CommonExampleOptions& options) +{ + return new GraspBox(options.m_guiHelper, options.m_option); +} diff --git a/examples/BulletRobotics/GraspBox.h b/examples/BulletRobotics/GraspBox.h new file mode 100644 index 000000000..70decb504 --- /dev/null +++ b/examples/BulletRobotics/GraspBox.h @@ -0,0 +1,21 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2016 Google Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef GRASP_BOX_EXAMPLE_H +#define GRASP_BOX_EXAMPLE_H + +class CommonExampleInterface* GraspBoxCreateFunc(struct CommonExampleOptions& options); + +#endif //GRASP_BOX_EXAMPLE_H diff --git a/examples/BulletRobotics/JointLimit.cpp b/examples/BulletRobotics/JointLimit.cpp index 1169b3039..6a4d91eef 100644 --- a/examples/BulletRobotics/JointLimit.cpp +++ b/examples/BulletRobotics/JointLimit.cpp @@ -51,20 +51,17 @@ public: b3RobotSimulatorSetPhysicsEngineParameters physicsArgs; physicsArgs.m_constraintSolverType = eConstraintSolverLCP_DANTZIG; - b3Printf(m_robotSim.getSolvername()); - physicsArgs.m_defaultGlobalCFM = 1e-6; - m_robotSim.setNumSolverIterations(10); - + m_robotSim.setNumSolverIterations(10); b3RobotSimulatorLoadUrdfFileArgs loadArgs; int humanoid = m_robotSim.loadURDF("test_joints_MB.urdf", loadArgs); - b3RobotSimulatorChangeDynamicsArgs dynamicsArgs; - dynamicsArgs.m_linearDamping = 0; - dynamicsArgs.m_angularDamping = 0; - m_robotSim.changeDynamics(humanoid, -1, dynamicsArgs); + b3RobotSimulatorChangeDynamicsArgs dynamicsArgs; + dynamicsArgs.m_linearDamping = 0; + dynamicsArgs.m_angularDamping = 0; + m_robotSim.changeDynamics(humanoid, -1, dynamicsArgs); m_robotSim.setGravity(btVector3(0, 0, -10)); } diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 137c2a71f..6dded9dba 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -148,6 +148,8 @@ SET(BulletExampleBrowser_SRCS ../BulletRobotics/BoxStack.cpp ../BulletRobotics/JointLimit.cpp + ../BulletRobotics/GraspBox.cpp + ../BulletRobotics/FixJointBoxes.cpp ../TinyRenderer/geometry.cpp ../TinyRenderer/model.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index c26197873..9693563fe 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -6,6 +6,8 @@ #include "../BulletRobotics/BoxStack.h" #include "../BulletRobotics/FixJointBoxes.h" #include "../BulletRobotics/JointLimit.h" +#include "../BulletRobotics/GraspBox.h" +#include "../BulletRobotics/FixJointBoxes.h" #include "../RenderingExamples/RenderInstancingDemo.h" #include "../RenderingExamples/CoordinateSystemDemo.h" #include "../RenderingExamples/RaytracerSetup.h" @@ -132,9 +134,12 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(1, "Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.", RigidBodySoftContactCreateFunc), + ExampleEntry(0, "Bullet Robotics"), ExampleEntry(1, "Box Stack", "Create a stack of boxes of large mass ratio.", BoxStackExampleCreateFunc), ExampleEntry(1, "Joint Limit", "Create three objects joint together", JointLimitCreateFunc), + ExampleEntry(1, "Grasp Box", "A robot arm of large mass tries to grasp a box of small mass", GraspBoxCreateFunc), + ExampleEntry(1, "FixJoint Boxes", "FixJoint Boxes", FixJointBoxesCreateFunc), ExampleEntry(0, "MultiBody"), ExampleEntry(1, "MultiDof", "Create a basic btMultiBody with 3-DOF spherical joints (mobilizers). The demo uses a fixed base or a floating base at restart.", MultiDofCreateFunc),