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

Load bunny through shared memory API and RobotSimAPI. Create grasp bunny example.

Browse Source
This commit is contained in:
yunfeibai
2016-10-17 13:01:04 -07:00
parent 880ee097fa
commit b07df4d504
10 changed files with 196 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files

167
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@@ -217,7 +217,6 @@ public:
if ((m_options & eONE_MOTOR_GRASP)!=0)
{
{
SliderParams slider("Vertical velocity",&sGripperVerticalVelocity);
slider.m_minVal=-2;
@@ -232,7 +231,7 @@ public:
slider.m_maxVal=1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
if (0)
if (1)
{
b3RobotSimLoadFileArgs args("");
b3RobotSimLoadFileResults results;
@@ -288,49 +287,6 @@ public:
}
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);
*/
/*
b3JointInfo revoluteJoint1;
revoluteJoint1.m_parentFrame[0] = -0.055;
revoluteJoint1.m_parentFrame[1] = 0;
@@ -371,9 +327,112 @@ public:
revoluteJoint2.m_jointType = ePoint2PointType;
m_robotSim.createJoint(1, 2, 1, 4, &revoluteJoint1);
m_robotSim.createJoint(1, 3, 1, 6, &revoluteJoint2);
*/
}
if ((m_options & eGRASP_SOFT_BODY)!=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);
}
if (1)
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "gripper/wsg50_one_motor_gripper_new.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));
m_robotSim.loadBunny();
b3JointInfo revoluteJoint1;
revoluteJoint1.m_parentFrame[0] = -0.055;
revoluteJoint1.m_parentFrame[1] = 0;
revoluteJoint1.m_parentFrame[2] = 0.02;
revoluteJoint1.m_parentFrame[3] = 0;
revoluteJoint1.m_parentFrame[4] = 0;
revoluteJoint1.m_parentFrame[5] = 0;
revoluteJoint1.m_parentFrame[6] = 1.0;
revoluteJoint1.m_childFrame[0] = 0;
revoluteJoint1.m_childFrame[1] = 0;
revoluteJoint1.m_childFrame[2] = 0;
revoluteJoint1.m_childFrame[3] = 0;
revoluteJoint1.m_childFrame[4] = 0;
revoluteJoint1.m_childFrame[5] = 0;
revoluteJoint1.m_childFrame[6] = 1.0;
revoluteJoint1.m_jointAxis[0] = 1.0;
revoluteJoint1.m_jointAxis[1] = 0.0;
revoluteJoint1.m_jointAxis[2] = 0.0;
revoluteJoint1.m_jointType = ePoint2PointType;
b3JointInfo revoluteJoint2;
revoluteJoint2.m_parentFrame[0] = 0.055;
revoluteJoint2.m_parentFrame[1] = 0;
revoluteJoint2.m_parentFrame[2] = 0.02;
revoluteJoint2.m_parentFrame[3] = 0;
revoluteJoint2.m_parentFrame[4] = 0;
revoluteJoint2.m_parentFrame[5] = 0;
revoluteJoint2.m_parentFrame[6] = 1.0;
revoluteJoint2.m_childFrame[0] = 0;
revoluteJoint2.m_childFrame[1] = 0;
revoluteJoint2.m_childFrame[2] = 0;
revoluteJoint2.m_childFrame[3] = 0;
revoluteJoint2.m_childFrame[4] = 0;
revoluteJoint2.m_childFrame[5] = 0;
revoluteJoint2.m_childFrame[6] = 1.0;
revoluteJoint2.m_jointAxis[0] = 1.0;
revoluteJoint2.m_jointAxis[1] = 0.0;
revoluteJoint2.m_jointAxis[2] = 0.0;
revoluteJoint2.m_jointType = ePoint2PointType;
m_robotSim.createJoint(0, 2, 0, 4, &revoluteJoint1);
m_robotSim.createJoint(0, 3, 0, 6, &revoluteJoint2);
}
}
virtual void exitPhysics()
{
@@ -402,6 +461,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);
}
}
if ((m_options & eGRASP_SOFT_BODY)!=0)
{
int fingerJointIndices[2]={0,1};
double fingerTargetVelocities[2]={sGripperVerticalVelocity,sGripperClosingTargetVelocity

1
examples/RoboticsLearning/GripperGraspExample.h
View File

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

2
examples/RoboticsLearning/KukaGraspExample.cpp
View File

@@ -256,7 +256,7 @@ public:
}
//m_robotSim.stepSimulation();
m_robotSim.stepSimulation();
}
virtual void renderScene()
{

6
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -998,4 +998,10 @@ void b3RobotSimAPI::getLinkState(int bodyUniqueId, int linkIndex, double* worldP
worldOrientation[2] = linkState.m_worldOrientation[2];
worldOrientation[3] = linkState.m_worldOrientation[3];
}
}
void b3RobotSimAPI::loadBunny()
{
b3SharedMemoryCommandHandle command = b3LoadBunnyCommandInit(m_data->m_physicsClient);
b3SubmitClientCommand(m_data->m_physicsClient, command);
}

2
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -164,6 +164,8 @@ public:
void getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian);
void getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition, double* worldOrientation);
void loadBunny();
};
#endif //B3_ROBOT_SIM_API_H