Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #716 from erwincoumans/master

Browse Source 
Add robotics learning add GripperGraspExample
This commit is contained in:
erwincoumans
2016-07-25 16:01:15 -07:00
committed by GitHub
parent 299243d49b 8270096fad
commit a6e5c6a273
17 changed files with 641 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -210,6 +210,8 @@ SET(BulletExampleBrowser_SRCS
../RenderingExamples/TimeSeriesCanvas.h
../RenderingExamples/TimeSeriesFontData.cpp
../RenderingExamples/TimeSeriesFontData.h
../RoboticsLearning/GripperGraspExample.cpp
../RoboticsLearning/GripperGraspExample.h
../RoboticsLearning/b3RobotSimAPI.cpp
../RoboticsLearning/b3RobotSimAPI.h
../RoboticsLearning/R2D2GraspExample.cpp

2
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -46,6 +46,7 @@
#include "../MultiThreading/MultiThreadingExample.h"
#include "../InverseDynamics/InverseDynamicsExample.h"
#include "../RoboticsLearning/R2D2GraspExample.h"
#include "../RoboticsLearning/GripperGraspExample.h"
#include "../InverseKinematics/InverseKinematicsExample.h"
#ifdef ENABLE_LUA
@@ -260,6 +261,7 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(1,"R2D2 Grasp","Load the R2D2 robot from URDF file and control it to grasp objects", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_GRASP),
ExampleEntry(1,"URDF Compliant Contact","Work-in-progress, experiment/improve compliant rigid contact using parameters from URDF file (contact_cfm, contact_erp, lateral_friction, rolling_friction)", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_COMPLIANT_CONTACT),
ExampleEntry(1,"Contact for Grasp","Grasp experiment to improve contact model", GripperGraspExampleCreateFunc),

2
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@@ -519,6 +519,8 @@ void MyStatusBarError(const char* msg)
gui2->textOutput(msg);
gui2->forceUpdateScrollBars();
}
btAssert(0);
}
struct MyMenuItemHander :public Gwen::Event::Handler

7
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -232,6 +232,13 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
if (mass)
{
compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
//temporary inertia scaling until we load inertia from URDF
if (contactInfo.m_flags & URDF_CONTACT_HAS_INERTIA_SCALING)
{
localInertiaDiagonal*=contactInfo.m_inertiaScaling;
}
}
btRigidBody* linkRigidBody = 0;

5
examples/Importers/ImportURDFDemo/URDFJointTypes.h
View File

@@ -17,7 +17,8 @@ enum URDF_LinkContactFlags
{
URDF_CONTACT_HAS_LATERAL_FRICTION=1,
URDF_CONTACT_HAS_ROLLING_FRICTION=2,
URDF_CONTACT_HAS_CONTACT_CFM=4,
URDF_CONTACT_HAS_INERTIA_SCALING=2,
URDF_CONTACT_HAS_CONTACT_CFM=4,
URDF_CONTACT_HAS_CONTACT_ERP=8
};
@@ -25,6 +26,7 @@ struct URDFLinkContactInfo
{
btScalar m_lateralFriction;
btScalar m_rollingFriction;
btScalar m_inertiaScaling;
btScalar m_contactCfm;
btScalar m_contactErp;
int m_flags;
@@ -32,6 +34,7 @@ struct URDFLinkContactInfo
URDFLinkContactInfo()
:m_lateralFriction(0.5),
m_rollingFriction(0),
m_inertiaScaling(1),
m_contactCfm(0),
m_contactErp(0)
{

23
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -606,6 +606,28 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
TiXmlElement* ci = config->FirstChildElement("contact");
if (ci)
{
TiXmlElement *damping_xml = ci->FirstChildElement("inertia_scaling");
if (damping_xml)
{
if (m_parseSDF)
{
link.m_contactInfo.m_inertiaScaling = urdfLexicalCast<double>(damping_xml->GetText());
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_INERTIA_SCALING;
} else
{
if (!damping_xml->Attribute("value"))
{
logger->reportError("Link/contact: damping element must have value attribute");
return false;
}
link.m_contactInfo.m_inertiaScaling = urdfLexicalCast<double>(damping_xml->Attribute("value"));
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_INERTIA_SCALING;
}
}
{
TiXmlElement *friction_xml = ci->FirstChildElement("lateral_friction");
if (friction_xml)
{
@@ -623,6 +645,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
link.m_contactInfo.m_lateralFriction = urdfLexicalCast<double>(friction_xml->Attribute("value"));
}
}
}
}
}

231
examples/RoboticsLearning/GripperGraspExample.cpp Normal file
View File

@@ -0,0 +1,231 @@
#include "GripperGraspExample.h"
#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
#include "Bullet3Common/b3Quaternion.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 <string>
#include "b3RobotSimAPI.h"
#include "../Utils/b3Clock.h"
static btScalar sGripperVerticalVelocity = -0.2f;
static btScalar sGripperClosingTargetVelocity = 0.5f;
class GripperGraspExample : public CommonExampleInterface
{
CommonGraphicsApp* m_app;
GUIHelperInterface* m_guiHelper;
b3RobotSimAPI m_robotSim;
int m_options;
int m_r2d2Index;
int m_gripperIndex;
float m_x;
float m_y;
float m_z;
b3AlignedObjectArray<int> m_movingInstances;
enum
{
numCubesX = 20,
numCubesY = 20
};
public:
GripperGraspExample(GUIHelperInterface* helper, int options)
:m_app(helper->getAppInterface()),
m_guiHelper(helper),
m_options(options),
m_r2d2Index(-1),
m_gripperIndex(-1),
m_x(0),
m_y(0),
m_z(0)
{
m_app->setUpAxis(2);
}
virtual ~GripperGraspExample()
{
m_app->m_renderer->enableBlend(false);
}
virtual void physicsDebugDraw(int debugDrawMode)
{
}
virtual void initPhysics()
{
bool connected = m_robotSim.connect(m_guiHelper);
b3Printf("robotSim connected = %d",connected);
{
{
SliderParams slider("Vertical velocity",&sGripperVerticalVelocity);
slider.m_minVal=-2;
slider.m_maxVal=2;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity
);
slider.m_minVal=-1;
slider.m_maxVal=1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "gripper/wsg50_with_r2d2_gripper.sdf";
args.m_fileType = B3_SDF_FILE;
b3RobotSimLoadFileResults results;
if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1)
{
m_gripperIndex = results.m_uniqueObjectIds[0];
int numJoints = m_robotSim.getNumJoints(m_gripperIndex);
b3Printf("numJoints = %d",numJoints);
for (int i=0;i<numJoints;i++)
{
b3JointInfo jointInfo;
m_robotSim.getJointInfo(m_gripperIndex,i,&jointInfo);
b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
}
/*
int fingerJointIndices[2]={1,3};
double fingerTargetPositions[2]={-0.04,0.04};
for (int i=0;i<2;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD);
controlArgs.m_targetPosition = fingerTargetPositions[i];
controlArgs.m_kp = 5.0;
controlArgs.m_kd = 3.0;
controlArgs.m_maxTorqueValue = 1.0;
m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
}
*/
int fingerJointIndices[3]={0,1,3};
double fingerTargetVelocities[3]={-0.2,.5,-.5};
double maxTorqueValues[3]={40.0,50.0,50.0};
for (int i=0;i<3;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = fingerTargetVelocities[i];
controlArgs.m_maxTorqueValue = maxTorqueValues[i];
controlArgs.m_kd = 1.;
m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
}
}
}
{
b3RobotSimLoadFileArgs args("");
b3RobotSimLoadFileResults results;
args.m_fileName = "cube_small.urdf";
args.m_startPosition.setValue(0,0,.107);
args.m_startOrientation.setEulerZYX(0,0,0);
m_robotSim.loadFile(args,results);
}
if (1)
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0);
args.m_startOrientation.setEulerZYX(0,0,0);
args.m_forceOverrideFixedBase = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
}
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
}
}
virtual void exitPhysics()
{
m_robotSim.disconnect();
}
virtual void stepSimulation(float deltaTime)
{
if ((m_gripperIndex>=0))
{
int fingerJointIndices[3]={0,1,3};
double fingerTargetVelocities[3]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
,-sGripperClosingTargetVelocity
};
double maxTorqueValues[3]={40.0,50.0,50.0};
for (int i=0;i<3;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = fingerTargetVelocities[i];
controlArgs.m_maxTorqueValue = maxTorqueValues[i];
controlArgs.m_kd = 1.;
m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
}
}
m_robotSim.stepSimulation();
}
virtual void renderScene()
{
m_robotSim.renderScene();
//m_app->m_renderer->renderScene();
}
virtual void physicsDebugDraw()
{
}
virtual bool mouseMoveCallback(float x,float y)
{
return false;
}
virtual bool mouseButtonCallback(int button, int state, float x, float y)
{
return false;
}
virtual bool keyboardCallback(int key, int state)
{
return false;
}
virtual void resetCamera()
{
float dist = 1.5;
float pitch = 12;
float yaw = -10;
float targetPos[3]={-0.2,0.8,0.3};
if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
{
m_app->m_renderer->getActiveCamera()->setCameraDistance(dist);
m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch);
m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw);
m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0],targetPos[1],targetPos[2]);
}
}
};
class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options)
{
return new GripperGraspExample(options.m_guiHelper, options.m_option);
}

23
examples/RoboticsLearning/GripperGraspExample.h Normal file
View File

@@ -0,0 +1,23 @@
/*
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 GRIPPER_GRASP_EXAMPLE_H
#define GRIPPER_GRASP_EXAMPLE_H
class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options);
#endif //GRIPPER_GRASP_EXAMPLE_H

2
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -560,6 +560,7 @@ void b3RobotSimAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const
b3JointInfo jointInfo;
b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo);
int uIndex = jointInfo.m_uIndex;
b3JointControlSetKd(command,uIndex,args.m_kd);
b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity);
b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
@@ -627,6 +628,7 @@ bool b3RobotSimAPI::loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotS
}
statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command);
statusType = b3GetStatusType(statusHandle);
b3Assert(statusType==CMD_URDF_LOADING_COMPLETED);
robotUniqueId = b3GetStatusBodyIndex(statusHandle);
results.m_uniqueObjectIds.push_back(robotUniqueId);

2
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -61,7 +61,7 @@ struct b3JointMotorArgs
m_targetPosition(0),
m_kp(0.1),
m_targetVelocity(0),
m_kd(0.1),
m_kd(0.9),
m_maxTorqueValue(1000)
{
}

21
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1216,16 +1216,21 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes);
//serverStatusOut.m_type = CMD_SDF_LOADING_FAILED;
serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size();
int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies);
for (int i=0;i<maxBodies;i++)
if (completedOk)
{
serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[i] = m_data->m_sdfRecentLoadedBodies[i];
}
//serverStatusOut.m_type = CMD_SDF_LOADING_FAILED;
serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size();
int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies);
for (int i=0;i<maxBodies;i++)
{
serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[i] = m_data->m_sdfRecentLoadedBodies[i];
}
serverStatusOut.m_type = CMD_SDF_LOADING_COMPLETED;
serverStatusOut.m_type = CMD_SDF_LOADING_COMPLETED;
} else
{
serverStatusOut.m_type = CMD_SDF_LOADING_FAILED;
}
hasStatus = true;
break;
}