From 1660957f75279cb3e094d342c140aa473df79985 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Thu, 7 Mar 2019 16:49:24 -0800 Subject: [PATCH] Untrack Grasp box example for now. This example needs more adjustment. fix compile --- examples/BulletRobotics/GraspBox.cpp | 186 --------------------- examples/BulletRobotics/GraspBox.h | 21 --- examples/ExampleBrowser/CMakeLists.txt | 2 +- examples/ExampleBrowser/ExampleEntries.cpp | 4 +- 4 files changed, 3 insertions(+), 210 deletions(-) delete mode 100644 examples/BulletRobotics/GraspBox.cpp delete mode 100644 examples/BulletRobotics/GraspBox.h diff --git a/examples/BulletRobotics/GraspBox.cpp b/examples/BulletRobotics/GraspBox.cpp deleted file mode 100644 index 6a8d077fa..000000000 --- a/examples/BulletRobotics/GraspBox.cpp +++ /dev/null @@ -1,186 +0,0 @@ - -#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 deleted file mode 100644 index 70decb504..000000000 --- a/examples/BulletRobotics/GraspBox.h +++ /dev/null @@ -1,21 +0,0 @@ -/* -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/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 6dded9dba..0b76fdead 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -148,7 +148,7 @@ SET(BulletExampleBrowser_SRCS ../BulletRobotics/BoxStack.cpp ../BulletRobotics/JointLimit.cpp - ../BulletRobotics/GraspBox.cpp + # ../BulletRobotics/GraspBox.cpp ../BulletRobotics/FixJointBoxes.cpp ../TinyRenderer/geometry.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 9693563fe..15080e4dc 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -6,7 +6,7 @@ #include "../BulletRobotics/BoxStack.h" #include "../BulletRobotics/FixJointBoxes.h" #include "../BulletRobotics/JointLimit.h" -#include "../BulletRobotics/GraspBox.h" +// #include "../BulletRobotics/GraspBox.h" #include "../BulletRobotics/FixJointBoxes.h" #include "../RenderingExamples/RenderInstancingDemo.h" #include "../RenderingExamples/CoordinateSystemDemo.h" @@ -138,7 +138,7 @@ static ExampleEntry gDefaultExamples[] = 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, "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"),