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

Merge pull request #812 from YunfeiBai/master

Browse Source 
Add a new one motor gripper.
This commit is contained in:
erwincoumans
2016-09-29 20:45:50 -07:00
committed by GitHub
parent f0d03c5751 67c49fa701
commit 3e0058e451
4 changed files with 836 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

386
data/gripper/wsg50_one_motor_gripper_new.sdf Normal file
View File

@@ -0,0 +1,386 @@
<?xml version="1.0" ?>
<sdf version='1.6'>
<world name='default'>
<model name='wsg50_with_gripper'>
<pose frame=''>0 0 0.26 3.14 0 0</pose>
<link name='world'>
</link>
<joint name='base_joint' type='prismatic'>
<parent>world</parent>
<child>base_link</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-0.5</lower>
<upper>10</upper>
<effort>1</effort>
<velocity>1</velocity>
</limit>
<dynamics>
<damping>0</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='base_link'>
<pose frame=''>0 0 0 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 0 0</pose>
<mass>1.2</mass>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
</inertial>
<visual name='base_link_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/WSG50_110.stl</uri>
</mesh>
</geometry>
<material>
</material>
</visual>
</link>
<link name='motor'>
<pose frame=''>0 0 0.03 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 0 0</pose>
<mass>0.1</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<visual name='motor_visual'>
<pose frame=''>0 0 0.01 0 0 0</pose>
<geometry>
<box>
<size>0.02 0.02 0.02 </size>
</box>
</geometry>
</visual>
</link>
<joint name='base_joint_motor' type='prismatic'>
<child>motor</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-0.055</lower>
<upper>0.001</upper>
<effort>10.0</effort>
<velocity>10.0</velocity>
</limit>
<dynamics>
<damping>0</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='left_hinge'>
<pose frame=''>0 0 0.04 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0.035 0 0 0</pose>
<mass>0.1</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<visual name='motor_visual'>
<pose frame=''>-0.03 0 0.01 0 -1.2 0</pose>
<geometry>
<box>
<size>0.02 0.02 0.07 </size>
</box>
</geometry>
</visual>
</link>
<joint name='motor_left_hinge_joint' type='revolute'>
<child>left_hinge</child>
<parent>motor</parent>
<axis>
<xyz>0 1 0</xyz>
<limit>
<lower>-20.0</lower>
<upper>20.0</upper>
<effort>10</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='right_hinge'>
<pose frame=''>0 0 0.04 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0.035 0 0 0</pose>
<mass>0.1</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<visual name='motor_visual'>
<pose frame=''>0.03 0 0.01 0 1.2 0</pose>
<geometry>
<box>
<size>0.02 0.02 0.07 </size>
</box>
</geometry>
</visual>
</link>
<joint name='motor_right_hinge_joint' type='revolute'>
<child>right_hinge</child>
<parent>motor</parent>
<axis>
<xyz>0 1 0</xyz>
<limit>
<lower>-20.0</lower>
<upper>20.0</upper>
<effort>10</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='gripper_left'>
<pose frame=''>-0.055 0 0.06 0 -0 0</pose>
<inertial>
<pose frame=''>0 0 0.0115 0 -0 0</pose>
<mass>0.2</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<visual name='gripper_left_visual'>
<pose frame=''>0 0 -0.06 0 0 0</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>meshes/GUIDE_WSG50_110.stl</uri>
</mesh>
</geometry>
</visual>
<visual name='gripper_left_fixed_joint_lump__finger_left_visual_1'>
<pose frame=''>0 0 -0.037 0 0 0</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>meshes/WSG-FMF.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='gripper_left_hinge_joint' type='prismatic'>
<child>gripper_left</child>
<parent>base_link</parent>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-0.01</lower>
<upper>0.05</upper>
<effort>1</effort>
<velocity>1</velocity>
</limit>
<dynamics>
<damping>0</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='gripper_right'>
<pose frame=''>0.055 0 0.06 0 0 3.14159</pose>
<inertial>
<pose frame=''>0 0 0.0115 0 -0 0</pose>
<mass>0.2</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<visual name='gripper_right_visual'>
<pose frame=''>0 0 -0.06 0 0 0</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>meshes/GUIDE_WSG50_110.stl</uri>
</mesh>
</geometry>
</visual>
<visual name='gripper_right_fixed_joint_lump__finger_right_visual_1'>
<pose frame=''>0 0 -0.037 0 0 0</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>meshes/WSG-FMF.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='gripper_right_hinge_joint' type='prismatic'>
<child>gripper_right</child>
<parent>base_link</parent>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-0.01</lower>
<upper>0.05</upper>
<effort>1</effort>
<velocity>1</velocity>
</limit>
<dynamics>
<damping>0</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='finger_right'>
<pose frame=''>0.062 0 0.145 0 0 1.5708</pose>
<inertial>
<mass>0.2</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<collision name='finger_right_collision'>
<pose frame=''>0 0 0.042 0 0 0 </pose>
<geometry>
<box>
<size>0.02 0.02 0.15 </size>
</box>
</geometry>
</collision>
<visual name='finger_right_visual'>
<pose frame=''>0 0 0 0 0 0 </pose>
<geometry>
<mesh>
<scale>1 1 1 </scale>
<uri>meshes/l_gripper_tip_scaled.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='gripper_finger_right' type='fixed'>
<parent>gripper_right</parent>
<child>finger_right</child>
</joint>
<link name='finger_left'>
<pose frame=''>-0.062 0 0.145 0 0 4.71239</pose>
<inertial>
<mass>0.2</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<collision name='finger_left_collision'>
<pose frame=''>0 0 0.042 0 0 0 </pose>
<geometry>
<box>
<size>0.02 0.02 0.15 </size>
</box>
</geometry>
</collision>
<visual name='finger_left_visual'>
<pose frame=''>0 0 0 0 0 0 </pose>
<geometry>
<mesh>
<scale>1 1 1 </scale>
<uri>meshes/l_gripper_tip_scaled.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='gripper_finger_left' type='fixed'>
<parent>gripper_left</parent>
<child>finger_left</child>
</joint>
</model>
</world>
</sdf>

383
data/gripper/wsg50_one_motor_gripper_new_free_base.sdf Normal file
View File

@@ -0,0 +1,383 @@
<?xml version="1.0" ?>
<sdf version='1.6'>
<world name='default'>
<model name='wsg50_with_gripper'>
<pose frame=''>0 -2.3 2.1 0 0 0</pose>
<link name='world'>
<pose frame=''>0 0 0 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 0 0</pose>
<mass>0.1</mass>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
</inertial>
</link>
<joint name='base_joint' type='fixed'>
<parent>world</parent>
<child>base_link</child>
</joint>
<link name='base_link'>
<pose frame=''>0 0 0 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 0 0</pose>
<mass>1.2</mass>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
</inertial>
<visual name='base_link_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/WSG50_110.stl</uri>
</mesh>
</geometry>
<material>
</material>
</visual>
</link>
<link name='motor'>
<pose frame=''>0 0 0.03 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 0 0</pose>
<mass>0.1</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<visual name='motor_visual'>
<pose frame=''>0 0 0.01 0 0 0</pose>
<geometry>
<box>
<size>0.02 0.02 0.02 </size>
</box>
</geometry>
</visual>
</link>
<joint name='base_joint_motor' type='prismatic'>
<child>motor</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-0.055</lower>
<upper>0.001</upper>
<effort>10.0</effort>
<velocity>10.0</velocity>
</limit>
<dynamics>
<damping>0</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='left_hinge'>
<pose frame=''>0 0 0.04 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0.035 0 0 0</pose>
<mass>0.1</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<visual name='motor_visual'>
<pose frame=''>-0.03 0 0.01 0 -1.2 0</pose>
<geometry>
<box>
<size>0.02 0.02 0.07 </size>
</box>
</geometry>
</visual>
</link>
<joint name='motor_left_hinge_joint' type='revolute'>
<child>left_hinge</child>
<parent>motor</parent>
<axis>
<xyz>0 1 0</xyz>
<limit>
<lower>-20.0</lower>
<upper>20.0</upper>
<effort>10</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='right_hinge'>
<pose frame=''>0 0 0.04 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0.035 0 0 0</pose>
<mass>0.1</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<visual name='motor_visual'>
<pose frame=''>0.03 0 0.01 0 1.2 0</pose>
<geometry>
<box>
<size>0.02 0.02 0.07 </size>
</box>
</geometry>
</visual>
</link>
<joint name='motor_right_hinge_joint' type='revolute'>
<child>right_hinge</child>
<parent>motor</parent>
<axis>
<xyz>0 1 0</xyz>
<limit>
<lower>-20.0</lower>
<upper>20.0</upper>
<effort>10</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='gripper_left'>
<pose frame=''>-0.055 0 0.06 0 -0 0</pose>
<inertial>
<pose frame=''>0 0 0.0115 0 -0 0</pose>
<mass>0.2</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<visual name='gripper_left_visual'>
<pose frame=''>0 0 -0.06 0 0 0</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>meshes/GUIDE_WSG50_110.stl</uri>
</mesh>
</geometry>
</visual>
<visual name='gripper_left_fixed_joint_lump__finger_left_visual_1'>
<pose frame=''>0 0 -0.037 0 0 0</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>meshes/WSG-FMF.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='gripper_left_hinge_joint' type='prismatic'>
<child>gripper_left</child>
<parent>base_link</parent>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-0.01</lower>
<upper>0.05</upper>
<effort>1</effort>
<velocity>1</velocity>
</limit>
<dynamics>
<damping>0</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='gripper_right'>
<pose frame=''>0.055 0 0.06 0 0 3.14159</pose>
<inertial>
<pose frame=''>0 0 0.0115 0 -0 0</pose>
<mass>0.2</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<visual name='gripper_right_visual'>
<pose frame=''>0 0 -0.06 0 0 0</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>meshes/GUIDE_WSG50_110.stl</uri>
</mesh>
</geometry>
</visual>
<visual name='gripper_right_fixed_joint_lump__finger_right_visual_1'>
<pose frame=''>0 0 -0.037 0 0 0</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>meshes/WSG-FMF.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='gripper_right_hinge_joint' type='prismatic'>
<child>gripper_right</child>
<parent>base_link</parent>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-0.01</lower>
<upper>0.05</upper>
<effort>1</effort>
<velocity>1</velocity>
</limit>
<dynamics>
<damping>0</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='finger_right'>
<pose frame=''>0.062 0 0.145 0 0 1.5708</pose>
<inertial>
<mass>0.2</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<collision name='finger_right_collision'>
<pose frame=''>0 0 0.042 0 0 0 </pose>
<geometry>
<box>
<size>0.02 0.02 0.15 </size>
</box>
</geometry>
</collision>
<visual name='finger_right_visual'>
<pose frame=''>0 0 0 0 0 0 </pose>
<geometry>
<mesh>
<scale>1 1 1 </scale>
<uri>meshes/l_gripper_tip_scaled.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='gripper_finger_right' type='fixed'>
<parent>gripper_right</parent>
<child>finger_right</child>
</joint>
<link name='finger_left'>
<pose frame=''>-0.062 0 0.145 0 0 4.71239</pose>
<inertial>
<mass>0.2</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<collision name='finger_left_collision'>
<pose frame=''>0 0 0.042 0 0 0 </pose>
<geometry>
<box>
<size>0.02 0.02 0.15 </size>
</box>
</geometry>
</collision>
<visual name='finger_left_visual'>
<pose frame=''>0 0 0 0 0 0 </pose>
<geometry>
<mesh>
<scale>1 1 1 </scale>
<uri>meshes/l_gripper_tip_scaled.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='gripper_finger_left' type='fixed'>
<parent>gripper_left</parent>
<child>finger_left</child>
</joint>
</model>
</world>
</sdf>

47
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@@ -235,7 +235,7 @@ public:
{
b3RobotSimLoadFileArgs args("");
b3RobotSimLoadFileResults results;
args.m_fileName = "cube_small.urdf";
args.m_fileName = "sphere_small.urdf";
args.m_startPosition.setValue(0, 0, .107);
args.m_startOrientation.setEulerZYX(0, 0, 0);
args.m_useMultiBody = true;
@@ -244,7 +244,7 @@ public:
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "gripper/wsg50_one_motor_gripper.sdf";
args.m_fileName = "gripper/wsg50_one_motor_gripper_new.sdf";
args.m_fileType = B3_SDF_FILE;
args.m_useMultiBody = true;
b3RobotSimLoadFileResults results;
@@ -287,6 +287,7 @@ public:
}
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
/*
b3JointInfo sliderJoint1;
sliderJoint1.m_parentFrame[0] = 0;
sliderJoint1.m_parentFrame[1] = 0;
@@ -327,6 +328,47 @@ public:
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;
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(1, 2, 1, 4, &revoluteJoint1);
m_robotSim.createJoint(1, 3, 1, 6, &revoluteJoint2);
}
}
@@ -372,6 +414,7 @@ public:
}
}
m_robotSim.stepSimulation();
}
virtual void renderScene()

37
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -12,6 +12,7 @@
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "LinearMath/btHashMap.h"
#include "BulletInverseDynamics/MultiBodyTree.hpp"
#include "IKTrajectoryHelper.h"
@@ -390,8 +391,8 @@ struct PhysicsServerCommandProcessorInternalData
btMultiBody* m_gripperMultiBody;
btMultiBodyFixedConstraint* m_kukaGripperFixed;
btMultiBody* m_kukaGripperMultiBody;
btMultiBodySliderConstraint* m_kukaGripperSlider1;
btMultiBodySliderConstraint* m_kukaGripperSlider2;
btMultiBodyPoint2Point* m_kukaGripperRevolute1;
btMultiBodyPoint2Point* m_kukaGripperRevolute2;
int m_huskyId;
int m_KukaId;
int m_sphereId;
@@ -2976,7 +2977,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
loadUrdf("r2d2.urdf", btVector3(2, -2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// Load one motor gripper for kuka
loadSdf("gripper/wsg50_one_motor_gripper_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
loadSdf("gripper/wsg50_one_motor_gripper_new_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
m_data->m_gripperId = bodyId + 1;
InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId);
InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId);
@@ -3002,23 +3003,23 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
//loadUrdf("nao/nao.urdf", btVector3(2,5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// Add slider joint for fingers
btVector3 pivotInParent1(0, 0, 0.06);
btVector3 pivotInParent1(-0.055, 0, 0.02);
btVector3 pivotInChild1(0, 0, 0);
btMatrix3x3 frameInParent1(btQuaternion(0, 0, 0, 1.0));
btMatrix3x3 frameInChild1(btQuaternion(0, 0, 0, 1.0));
btVector3 jointAxis1(1.0, 0, 0);
btVector3 pivotInParent2(0, 0, 0.06);
btVector3 pivotInParent2(0.055, 0, 0.02);
btVector3 pivotInChild2(0, 0, 0);
btMatrix3x3 frameInParent2(btQuaternion(0, 0, 0, 1.0));
btMatrix3x3 frameInChild2(btQuaternion(0, 0, 1.0, 0));
btVector3 jointAxis2(1.0, 0, 0);
m_data->m_kukaGripperSlider1 = new btMultiBodySliderConstraint(gripperBody->m_multiBody, 0, gripperBody->m_multiBody, 3, pivotInParent1, pivotInChild1, frameInParent1, frameInChild1, jointAxis1);
m_data->m_kukaGripperSlider1->setMaxAppliedImpulse(5.0);
m_data->m_kukaGripperSlider2 = new btMultiBodySliderConstraint(gripperBody->m_multiBody, 0, gripperBody->m_multiBody, 6, pivotInParent2, pivotInChild2, frameInParent2, frameInChild2, jointAxis2);
m_data->m_kukaGripperSlider2->setMaxAppliedImpulse(5.0);
m_data->m_kukaGripperRevolute1 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 2, gripperBody->m_multiBody, 4, pivotInParent1, pivotInChild1);
m_data->m_kukaGripperRevolute1->setMaxAppliedImpulse(5.0);
m_data->m_kukaGripperRevolute2 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 3, gripperBody->m_multiBody, 6, pivotInParent2, pivotInChild2);
m_data->m_kukaGripperRevolute2->setMaxAppliedImpulse(5.0);
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperSlider1);
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperSlider2);
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute1);
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute2);
if (kukaBody->m_multiBody)
{
@@ -3075,8 +3076,8 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
// Table area
loadUrdf("table.urdf", objectWorldTr[0].getOrigin(), objectWorldTr[0].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("tray.urdf", objectWorldTr[1].getOrigin(), objectWorldTr[1].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// loadUrdf("cup_small.urdf", objectWorldTr[2].getOrigin(), objectWorldTr[2].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// loadUrdf("pitcher_small.urdf", objectWorldTr[3].getOrigin(), objectWorldTr[3].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("cup_small.urdf", objectWorldTr[2].getOrigin(), objectWorldTr[2].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("pitcher_small.urdf", objectWorldTr[3].getOrigin(), objectWorldTr[3].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("teddy_vhacd.urdf", objectWorldTr[4].getOrigin(), objectWorldTr[4].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("cube_small.urdf", objectWorldTr[5].getOrigin(), objectWorldTr[5].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere_small.urdf", objectWorldTr[6].getOrigin(), objectWorldTr[6].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
@@ -3090,7 +3091,13 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
// Chess area
loadUrdf("table_square.urdf", btVector3(2.0, 0, 0.0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("pawn.urdf", btVector3(1.8, -0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("queen.urdf", btVector3(1.9, -0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("king.urdf", btVector3(2.0, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("bishop.urdf", btVector3(2.1, 0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("rook.urdf", btVector3(2.2, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("knight.urdf", btVector3(2.2, 0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("husky/husky.urdf", btVector3(5, 2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_huskyId = bodyId;
@@ -3105,7 +3112,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)childBody->m_multiBody->getLink(1).m_userPtr;
if (motor)
{
btScalar posTarget = (-0.045)*btMin(btScalar(0.75), gVRGripperAnalog) / 0.75;
btScalar posTarget = (-0.048)*btMin(btScalar(0.75), gVRGripperAnalog) / 0.75;
motor->setPositionTarget(posTarget, .2);
motor->setVelocityTarget(0.0, .5);
motor->setMaxAppliedImpulse(5.0);