Attach gripper to kuka arm.
This commit is contained in:
YunfeiBai
@@ -385,4 +385,4 @@
|
||||
</joint>
|
||||
</model>
|
||||
</world>
|
||||
</sdf>
|
||||
</sdf>
|
||||
365
data/gripper/wsg50_one_motor_gripper_free.sdf
Normal file
365
data/gripper/wsg50_one_motor_gripper_free.sdf
Normal file
@@ -0,0 +1,365 @@
|
||||
<?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='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.047</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='revolute'>
|
||||
<child>gripper_left</child>
|
||||
<parent>left_hinge</parent>
|
||||
<axis>
|
||||
<xyz>0 1 0</xyz>
|
||||
<limit>
|
||||
<lower>-1.0</lower>
|
||||
<upper>1.0</upper>
|
||||
<effort>10</effort>
|
||||
<velocity>10</velocity>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<damping>0.01</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_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='revolute'>
|
||||
<child>gripper_right</child>
|
||||
<parent>right_hinge</parent>
|
||||
<axis>
|
||||
<xyz>0 1 0</xyz>
|
||||
<limit>
|
||||
<lower>-1.0</lower>
|
||||
<upper>1.0</upper>
|
||||
<effort>10</effort>
|
||||
<velocity>10</velocity>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<damping>0.01</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='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>
|
||||
142
data/pr2_gripper_short.urdf
Normal file
142
data/pr2_gripper_short.urdf
Normal file
@@ -0,0 +1,142 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot name="physics">
|
||||
<link name="gripper_pole">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.04" radius=".01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0 " xyz="0.02 0 0"/>
|
||||
<material name="Gray">
|
||||
<color rgba=".7 .7 .7 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.04" radius=".01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0 " xyz="0.02 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_gripper_joint" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.02 0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="left_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_gripper">
|
||||
<contact>
|
||||
<lateral_friction value="1.0"/>
|
||||
<spinning_friction value="1.5"/>
|
||||
</contact>
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="l_finger_collision.stl"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="l_finger_collision.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_tip_joint" type="fixed">
|
||||
<parent link="left_gripper"/>
|
||||
<child link="left_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_tip">
|
||||
<contact>
|
||||
<lateral_friction value="1.0"/>
|
||||
<spinning_friction value="1.5"/>
|
||||
</contact>
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="l_finger_tip.stl"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size=".03 .03 .02"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 0 0" xyz="0.105 0.00495 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_gripper_joint" type="revolute">
|
||||
<axis xyz="0 0 -1"/>
|
||||
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.02 -0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="right_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_gripper">
|
||||
<contact>
|
||||
<lateral_friction value="1.0"/>
|
||||
<spinning_friction value="1.5"/>
|
||||
</contact>
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="l_finger_collision.stl"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="l_finger_collision.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_tip_joint" type="fixed">
|
||||
<parent link="right_gripper"/>
|
||||
<child link="right_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_tip">
|
||||
<contact>
|
||||
<lateral_friction value="1.0"/>
|
||||
<spinning_friction value="1.5"/>
|
||||
</contact>
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="l_finger_tip.stl"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size=".03 .03 .02"/>
|
||||
</geometry>
|
||||
<origin rpy="-3.1415 0 0" xyz="0.105 0.00495 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
</robot>
|
||||
@@ -388,9 +388,12 @@ struct PhysicsServerCommandProcessorInternalData
|
||||
|
||||
btMultiBodyFixedConstraint* m_gripperRigidbodyFixed;
|
||||
btMultiBody* m_gripperMultiBody;
|
||||
btMultiBodyFixedConstraint* m_kukaGripperFixed;
|
||||
btMultiBody* m_kukaGripperMultiBody;
|
||||
int m_huskyId;
|
||||
int m_KukaId;
|
||||
int m_sphereId;
|
||||
int m_gripperId;
|
||||
CommandLogger* m_commandLogger;
|
||||
CommandLogPlayback* m_logPlayback;
|
||||
|
||||
@@ -445,6 +448,7 @@ struct PhysicsServerCommandProcessorInternalData
|
||||
m_huskyId(-1),
|
||||
m_KukaId(-1),
|
||||
m_sphereId(-1),
|
||||
m_gripperId(-1),
|
||||
m_commandLogger(0),
|
||||
m_logPlayback(0),
|
||||
m_physicsDeltaTime(1./240.),
|
||||
@@ -2945,13 +2949,93 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
|
||||
loadUrdf("sphere2.urdf", btVector3(-5, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
|
||||
loadUrdf("sphere2.urdf", btVector3(-5, 0, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
|
||||
loadUrdf("sphere2.urdf", btVector3(-5, 0, 3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
|
||||
loadUrdf("r2d2.urdf", btVector3(2, -2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
|
||||
|
||||
loadUrdf("r2d2.urdf", btVector3(2, -2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
|
||||
|
||||
loadUrdf("kuka_iiwa/model.urdf", btVector3(2.0, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
|
||||
m_data->m_KukaId = bodyId;
|
||||
loadUrdf("pr2_gripper.urdf", btVector3(2.2, 0.1, 0.1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
|
||||
|
||||
|
||||
loadSdf("gripper/wsg50_one_motor_gripper_free.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
|
||||
m_data->m_gripperId = bodyId + 1;
|
||||
InteralBodyData* parentBody = m_data->getHandle(m_data->m_KukaId);
|
||||
InteralBodyData* childBody = m_data->getHandle(m_data->m_gripperId);
|
||||
|
||||
// Reset the default gripper motor maximum torque for damping to 0
|
||||
for (int i = 0; i < childBody->m_multiBody->getNumLinks(); i++)
|
||||
{
|
||||
if (supportsJointMotor(childBody->m_multiBody, i))
|
||||
{
|
||||
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)childBody->m_multiBody->getLink(i).m_userPtr;
|
||||
if (motor)
|
||||
{
|
||||
motor->setMaxAppliedImpulse(0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Add slider joint for fingers
|
||||
btVector3 pivotInParent1(0, 0, 0.06);
|
||||
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 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);
|
||||
btMultiBodySliderConstraint* multibodySlider1 = new btMultiBodySliderConstraint(childBody->m_multiBody, 0, childBody->m_multiBody, 2, pivotInParent1, pivotInChild1, frameInParent1, frameInChild1, jointAxis1);
|
||||
multibodySlider1->setMaxAppliedImpulse(500.0);
|
||||
btMultiBodySliderConstraint* multibodySlider2 = new btMultiBodySliderConstraint(childBody->m_multiBody, 0, childBody->m_multiBody, 5, pivotInParent2, pivotInChild2, frameInParent2, frameInChild2, jointAxis2);
|
||||
multibodySlider1->setMaxAppliedImpulse(500.0);
|
||||
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
|
||||
world->addMultiBodyConstraint(multibodySlider1);
|
||||
world->addMultiBodyConstraint(multibodySlider2);
|
||||
|
||||
if (parentBody->m_multiBody)
|
||||
{
|
||||
childBody->m_multiBody->setHasSelfCollision(0);
|
||||
btVector3 pivotInParent(0, 0, 0.02);
|
||||
btMatrix3x3 frameInParent;
|
||||
frameInParent.setIdentity();
|
||||
btVector3 pivotInChild(0, 0, 0);
|
||||
btMatrix3x3 frameInChild;
|
||||
frameInChild.setIdentity();
|
||||
|
||||
m_data->m_kukaGripperFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody, 6, childBody->m_multiBody, 0, pivotInParent, pivotInChild, frameInParent, frameInChild);
|
||||
m_data->m_kukaGripperMultiBody = childBody->m_multiBody;
|
||||
m_data->m_kukaGripperFixed->setMaxAppliedImpulse(10000);
|
||||
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
|
||||
world->addMultiBodyConstraint(m_data->m_kukaGripperFixed);
|
||||
}
|
||||
|
||||
// Load pr2 gripper for kuka
|
||||
/*
|
||||
loadUrdf("pr2_gripper_short.urdf", btVector3(2.2, 0.1, 0.1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
|
||||
InteralBodyData* parentBody = m_data->getHandle(m_data->m_KukaId);
|
||||
InteralBodyData* childBody = m_data->getHandle(bodyId);
|
||||
if (parentBody->m_multiBody)
|
||||
{
|
||||
btVector3 pivotInParent(0, 0, 0.04);
|
||||
btMatrix3x3 frameInParent;
|
||||
frameInParent.setIdentity();
|
||||
btVector3 pivotInChild(0, 0, 0);
|
||||
btMatrix3x3 frameInChild;
|
||||
frameInChild.setRotation(btQuaternion(0, 0.70711, 0, 0.70711));
|
||||
|
||||
m_data->m_kukaGripperFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody, 6, childBody->m_multiBody, 0, pivotInParent, pivotInChild, frameInParent, frameInChild);
|
||||
m_data->m_kukaGripperMultiBody = childBody->m_multiBody;
|
||||
if (m_data->m_kukaGripperMultiBody->getNumLinks() > 2)
|
||||
{
|
||||
m_data->m_kukaGripperMultiBody->setJointPos(0, 0);
|
||||
m_data->m_kukaGripperMultiBody->setJointPos(2, 0);
|
||||
}
|
||||
m_data->m_kukaGripperFixed->setMaxAppliedImpulse(10000);
|
||||
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
|
||||
world->addMultiBodyConstraint(m_data->m_kukaGripperFixed);
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
loadUrdf("cube_small.urdf", btVector3(0.3, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user