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

Create a demo for one motor gripper grasp.

Browse Source
This commit is contained in:
yunfeibai
2016-09-21 12:08:03 -07:00
parent bb214e823b
commit 27fab2adb5
6 changed files with 528 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -266,7 +266,7 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(1,"Rolling friction","Experiment on multibody rolling friction", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_ROLLING_FRICTION),
ExampleEntry(1,"Gripper Grasp","Grasp experiment with a gripper to improve contact model", GripperGraspExampleCreateFunc,eGRIPPER_GRASP),
ExampleEntry(1,"Two Point Grasp","Grasp experiment with two point contact to test rolling friction", GripperGraspExampleCreateFunc, eTWO_POINT_GRASP),
ExampleEntry(1,"One Motor Gripper Grasp","Grasp experiment with a gripper with one motor to test slider constraint for closed loop structure", GripperGraspExampleCreateFunc, eONE_MOTOR_GRASP),
#ifdef ENABLE_LUA

131
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@@ -10,6 +10,7 @@
#include "../SharedMemory/PhysicsServerSharedMemory.h"
#include "../SharedMemory/PhysicsClientC_API.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "../SharedMemory/SharedMemoryPublic.h"
#include <string>
#include "b3RobotSimAPI.h"
@@ -213,6 +214,120 @@ public:
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
m_robotSim.setNumSimulationSubSteps(4);
}
if ((m_options & eONE_MOTOR_GRASP)!=0)
{
{
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("");
b3RobotSimLoadFileResults results;
args.m_fileName = "cube_small.urdf";
args.m_startPosition.setValue(0, 0, .107);
args.m_startOrientation.setEulerZYX(0, 0, 0);
args.m_useMultiBody = true;
m_robotSim.loadFile(args, results);
}
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "gripper/wsg50_one_motor_gripper.sdf";
args.m_fileType = B3_SDF_FILE;
args.m_useMultiBody = true;
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);
}
for (int i=0;i<8;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_maxTorqueValue = 0.0;
m_robotSim.setJointMotorControl(m_gripperIndex,i,controlArgs);
}
}
}
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;
args.m_useMultiBody = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
}
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
b3JointInfo sliderJoint1;
sliderJoint1.m_parentFrame[0] = 0;
sliderJoint1.m_parentFrame[1] = 0;
sliderJoint1.m_parentFrame[2] = 0.06;
sliderJoint1.m_parentFrame[3] = 0;
sliderJoint1.m_parentFrame[4] = 0;
sliderJoint1.m_parentFrame[5] = 0;
sliderJoint1.m_parentFrame[6] = 1.0;
sliderJoint1.m_childFrame[0] = 0;
sliderJoint1.m_childFrame[1] = 0;
sliderJoint1.m_childFrame[2] = 0;
sliderJoint1.m_childFrame[3] = 0;
sliderJoint1.m_childFrame[4] = 0;
sliderJoint1.m_childFrame[5] = 0;
sliderJoint1.m_childFrame[6] = 1.0;
sliderJoint1.m_jointAxis[0] = 1.0;
sliderJoint1.m_jointAxis[1] = 0.0;
sliderJoint1.m_jointAxis[2] = 0.0;
sliderJoint1.m_jointType = ePrismaticType;
b3JointInfo sliderJoint2;
sliderJoint2.m_parentFrame[0] = 0;
sliderJoint2.m_parentFrame[1] = 0;
sliderJoint2.m_parentFrame[2] = 0.06;
sliderJoint2.m_parentFrame[3] = 0;
sliderJoint2.m_parentFrame[4] = 0;
sliderJoint2.m_parentFrame[5] = 0;
sliderJoint2.m_parentFrame[6] = 1.0;
sliderJoint2.m_childFrame[0] = 0;
sliderJoint2.m_childFrame[1] = 0;
sliderJoint2.m_childFrame[2] = 0;
sliderJoint2.m_childFrame[3] = 0;
sliderJoint2.m_childFrame[4] = 0;
sliderJoint2.m_childFrame[5] = 1.0;
sliderJoint2.m_childFrame[6] = 0;
sliderJoint2.m_jointAxis[0] = 1.0;
sliderJoint2.m_jointAxis[1] = 0.0;
sliderJoint2.m_jointAxis[2] = 0.0;
sliderJoint2.m_jointType = ePrismaticType;
m_robotSim.createJoint(1, 0, 1, 3, &sliderJoint1);
m_robotSim.createJoint(1, 0, 1, 6, &sliderJoint2);
}
}
virtual void exitPhysics()
@@ -241,6 +356,22 @@ public:
}
}
if ((m_options & eONE_MOTOR_GRASP)!=0)
{
int fingerJointIndices[2]={0,1};
double fingerTargetVelocities[2]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
};
double maxTorqueValues[2]={50.0,50.0};
for (int i=0;i<2;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()

1
examples/RoboticsLearning/GripperGraspExample.h
View File

@@ -20,6 +20,7 @@ enum GripperGraspExampleOptions
{
eGRIPPER_GRASP=1,
eTWO_POINT_GRASP=2,
eONE_MOTOR_GRASP=4,
};
class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options);

12
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -2502,34 +2502,34 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_createJointArguments.m_parentFrame[3], clientCmd.m_createJointArguments.m_parentFrame[4], clientCmd.m_createJointArguments.m_parentFrame[5], clientCmd.m_createJointArguments.m_parentFrame[6]));
btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_createJointArguments.m_childFrame[3], clientCmd.m_createJointArguments.m_childFrame[4], clientCmd.m_createJointArguments.m_childFrame[5], clientCmd.m_createJointArguments.m_childFrame[6]));
btVector3 jointAxis(clientCmd.m_createJointArguments.m_jointAxis[0], clientCmd.m_createJointArguments.m_jointAxis[1], clientCmd.m_createJointArguments.m_jointAxis[2]);
if (clientCmd.m_createJointArguments.m_jointType == btMultibodyLink::eFixed)
if (clientCmd.m_createJointArguments.m_jointType == eFixedType)
{
if (childBody->m_multiBody)
{
btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild);
multibodyFixed->setMaxAppliedImpulse(2.0);
multibodyFixed->setMaxAppliedImpulse(500.0);
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed);
}
else
{
btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild);
rigidbodyFixed->setMaxAppliedImpulse(2.0);
rigidbodyFixed->setMaxAppliedImpulse(500.0);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(rigidbodyFixed);
}
}
else if (clientCmd.m_createJointArguments.m_jointType == btMultibodyLink::ePrismatic)
else if (clientCmd.m_createJointArguments.m_jointType == ePrismaticType)
{
if (childBody->m_multiBody)
{
btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
multibodySlider->setMaxAppliedImpulse(2.0);
multibodySlider->setMaxAppliedImpulse(500.0);
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider);
}
else
{
btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
rigidbodySlider->setMaxAppliedImpulse(2.0);
rigidbodySlider->setMaxAppliedImpulse(500.0);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(rigidbodySlider);
}

1
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -98,6 +98,7 @@ enum
enum JointType {
eRevoluteType = 0,
ePrismaticType = 1,
eFixedType = 2,
};
struct b3JointInfo