mirror of
https://github.com/bulletphysics/bullet3
synced 2025-01-05 15:21:06 +00:00
add nicer meshes to kuka_with_gripper.sdf and add kuka_with_gripper2.sdf that can rotate without messing up IK
fix tray/tray_textured4.obj and tray/tray.urdf fix kuka_with_cube.py allow both IK /end-effector control and joint-space control in kuka environment, use 1./240. sec. step and 150 solver iter bump up pybullet to 1.1.7
This commit is contained in:
erwincoumans
parent
cc34ebab25
commit
d2888f0884
@ -38,13 +38,13 @@
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>meshes/link_0.stl</uri>
|
||||
<uri>meshes/link_0.obj</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.5 0.5 0.5 1</diffuse>
|
||||
<specular>0.1 0.1 0.1 1</specular>
|
||||
<diffuse>0.2 0.2 0.2 1.0</diffuse>
|
||||
<specular>0.4 0.4 0.4 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
@ -77,13 +77,13 @@
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>meshes/link_1.stl</uri>
|
||||
<uri>meshes/link_1.obj</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.5 0.5 0.5 1</diffuse>
|
||||
<specular>0.1 0.1 0.1 1</specular>
|
||||
<diffuse>0.5 0.7 1.0 1.0</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
@ -135,13 +135,13 @@
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>meshes/link_2.stl</uri>
|
||||
<uri>meshes/link_2.obj</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>1.0 0.42 0.04 1</diffuse>
|
||||
<specular>0.1 0.1 0.1 1</specular>
|
||||
<diffuse>0.5 0.7 1.0 1.0</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
@ -193,13 +193,13 @@
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>meshes/link_3.stl</uri>
|
||||
<uri>meshes/link_3.obj</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.1 0.1 0.1 1</specular>
|
||||
<diffuse>1.0 0.423529411765 0.0392156862745 1.0</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
@ -251,13 +251,13 @@
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>meshes/link_4.stl</uri>
|
||||
<uri>meshes/link_4.obj</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.1 0.1 0.1 1</specular>
|
||||
<diffuse>0.5 0.7 1.0 1.0</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
@ -309,13 +309,13 @@
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>meshes/link_5.stl</uri>
|
||||
<uri>meshes/link_5.obj</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.1 0.1 0.1 1</specular>
|
||||
<diffuse>0.5 0.7 1.0 1.0</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
@ -367,13 +367,13 @@
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>meshes/link_6.stl</uri>
|
||||
<uri>meshes/link_6.obj</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>1.0 0.42 0.04 1</diffuse>
|
||||
<specular>0.1 0.1 0.1 1</specular>
|
||||
<diffuse>1.0 0.423529411765 0.0392156862745 1.0</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
@ -425,13 +425,13 @@
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>meshes/link_7.stl</uri>
|
||||
<uri>meshes/link_7.obj</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.1 0.1 0.1 1</specular>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
@ -482,6 +482,12 @@
|
||||
<size>0.05 0.05 0.1 </size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
@ -491,8 +497,8 @@
|
||||
<axis>
|
||||
<xyz>0 1 0</xyz>
|
||||
<limit>
|
||||
<lower>-0.4</lower>
|
||||
<upper>0.01</upper>
|
||||
<lower>-10.4</lower>
|
||||
<upper>10.01</upper>
|
||||
<effort>100</effort>
|
||||
<velocity>0</velocity>
|
||||
</limit>
|
||||
@ -525,6 +531,12 @@
|
||||
<size>0.01 0.01 0.08</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name='left_finger_base_joint' type='fixed'>
|
||||
@ -553,6 +565,12 @@
|
||||
<uri>meshes/finger_base_left.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
<collision name='left_finger_base_collision'>
|
||||
<pose frame=''>0 0 0 0 0 0 </pose>
|
||||
@ -570,8 +588,8 @@
|
||||
<axis>
|
||||
<xyz>0 1 0</xyz>
|
||||
<limit>
|
||||
<lower>-0.1</lower>
|
||||
<upper>0.3</upper>
|
||||
<lower>-10.1</lower>
|
||||
<upper>10.3</upper>
|
||||
<effort>0</effort>
|
||||
<velocity>0</velocity>
|
||||
</limit>
|
||||
@ -609,6 +627,12 @@
|
||||
<uri>meshes/finger_tip_left.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
<collision name='left_finger_tip_collision'>
|
||||
<pose frame=''>0 0 0 0 0 0</pose>
|
||||
@ -626,8 +650,8 @@
|
||||
<axis>
|
||||
<xyz>0 1 0</xyz>
|
||||
<limit>
|
||||
<lower>-0.01</lower>
|
||||
<upper>0.4</upper>
|
||||
<lower>-10.01</lower>
|
||||
<upper>10.4</upper>
|
||||
<effort>100</effort>
|
||||
<velocity>0</velocity>
|
||||
</limit>
|
||||
@ -660,6 +684,12 @@
|
||||
<size>0.01 0.01 0.08</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name='right_finger_base_joint' type='fixed'>
|
||||
@ -688,6 +718,12 @@
|
||||
<uri>meshes/finger_base_right.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
<collision name='right_finger_base_collision'>
|
||||
<pose frame=''>0 0 0 0 0 0 </pose>
|
||||
@ -705,8 +741,8 @@
|
||||
<axis>
|
||||
<xyz>0 1 0</xyz>
|
||||
<limit>
|
||||
<lower>-0.3</lower>
|
||||
<upper>0.1</upper>
|
||||
<lower>-10.3</lower>
|
||||
<upper>10.1</upper>
|
||||
<effort>0</effort>
|
||||
<velocity>0</velocity>
|
||||
</limit>
|
||||
@ -744,6 +780,12 @@
|
||||
<uri>meshes/finger_tip_right.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
<collision name='right_finger_tip_collision'>
|
||||
<pose frame=''>0 0 0 0 0 0</pose>
|
||||
|
||||
814
data/kuka_iiwa/kuka_with_gripper2.sdf
Normal file
814
data/kuka_iiwa/kuka_with_gripper2.sdf
Normal file
@ -0,0 +1,814 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!--This file contains the SDF model of a KUKA iiwa robot with a wsg50 gripper.
|
||||
It has been produced from the varients in //third_party/robotics/models.
|
||||
Note: This file is temporary, and should be deleted once Bullet supports
|
||||
importing models in SDF. Also, this file has been specialized for Bullet,
|
||||
because the mass of the base link has been set to 0, as needed by Bullet.
|
||||
Note: All of the gripper link poses have been adjusted in the z direction
|
||||
to achieve a reasonable position of the gripper relative to the arm.
|
||||
Note: The joint names for the KUKA have been changed to J0, J1, etc. -->
|
||||
<sdf version='1.6'>
|
||||
<world name='default'>
|
||||
<model name='lbr_iiwa_with_wsg50'>
|
||||
<link name='lbr_iiwa_link_0'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<inertial>
|
||||
<pose frame=''>-0.1 0 0.07 0 -0 0</pose>
|
||||
<mass>0</mass>
|
||||
<inertia>
|
||||
<ixx>0.05</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.06</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.03</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='lbr_iiwa_link_0_collision'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>/meshes/link_0.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name='lbr_iiwa_link_0_visual'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>meshes/link_0.obj</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.2 0.2 0.2 1.0</diffuse>
|
||||
<specular>0.4 0.4 0.4 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name='lbr_iiwa_link_1'>
|
||||
<pose frame=''>0 0 0.1575 0 -0 0</pose>
|
||||
<inertial>
|
||||
<pose frame=''>0 -0.03 0.12 0 -0 0</pose>
|
||||
<mass>4</mass>
|
||||
<inertia>
|
||||
<ixx>0.1</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.09</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.02</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='lbr_iiwa_link_1_collision'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>meshes/link_1.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name='lbr_iiwa_link_1_visual'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>meshes/link_1.obj</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.5 0.7 1.0 1.0</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name='J0' type='revolute'>
|
||||
<child>lbr_iiwa_link_1</child>
|
||||
<parent>lbr_iiwa_link_0</parent>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<lower>-2.96706</lower>
|
||||
<upper>2.96706</upper>
|
||||
<effort>300</effort>
|
||||
<velocity>10</velocity>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<damping>0.5</damping>
|
||||
<friction>0</friction>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name='lbr_iiwa_link_2'>
|
||||
<pose frame=''>0 0 0.36 1.5708 -0 -3.14159</pose>
|
||||
<inertial>
|
||||
<pose frame=''>0.0003 0.059 0.042 0 -0 0</pose>
|
||||
<mass>4</mass>
|
||||
<inertia>
|
||||
<ixx>0.05</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.018</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.044</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='lbr_iiwa_link_2_collision'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>meshes/link_2.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name='lbr_iiwa_link_2_visual'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>meshes/link_2.obj</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.5 0.7 1.0 1.0</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name='J1' type='revolute'>
|
||||
<child>lbr_iiwa_link_2</child>
|
||||
<parent>lbr_iiwa_link_1</parent>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<lower>-2.0944</lower>
|
||||
<upper>2.0944</upper>
|
||||
<effort>300</effort>
|
||||
<velocity>10</velocity>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<damping>0.5</damping>
|
||||
<friction>0</friction>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name='lbr_iiwa_link_3'>
|
||||
<pose frame=''>0 -0 0.5645 0 0 0</pose>
|
||||
<inertial>
|
||||
<pose frame=''>0 0.03 0.13 0 -0 0</pose>
|
||||
<mass>3</mass>
|
||||
<inertia>
|
||||
<ixx>0.08</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.075</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.01</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='lbr_iiwa_link_3_collision'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>meshes/link_3.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name='lbr_iiwa_link_3_visual'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>meshes/link_3.obj</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>1.0 0.423529411765 0.0392156862745 1.0</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name='J2' type='revolute'>
|
||||
<child>lbr_iiwa_link_3</child>
|
||||
<parent>lbr_iiwa_link_2</parent>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<lower>-2.96706</lower>
|
||||
<upper>2.96706</upper>
|
||||
<effort>300</effort>
|
||||
<velocity>10</velocity>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<damping>0.5</damping>
|
||||
<friction>0</friction>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name='lbr_iiwa_link_4'>
|
||||
<pose frame=''>0 -0 0.78 1.5708 0 0</pose>
|
||||
<inertial>
|
||||
<pose frame=''>0 0.067 0.034 0 -0 0</pose>
|
||||
<mass>2.7</mass>
|
||||
<inertia>
|
||||
<ixx>0.03</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.01</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.029</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='lbr_iiwa_link_4_collision'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>meshes/link_4.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name='lbr_iiwa_link_4_visual'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>meshes/link_4.obj</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.5 0.7 1.0 1.0</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name='J3' type='revolute'>
|
||||
<child>lbr_iiwa_link_4</child>
|
||||
<parent>lbr_iiwa_link_3</parent>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<lower>-2.0944</lower>
|
||||
<upper>2.0944</upper>
|
||||
<effort>300</effort>
|
||||
<velocity>10</velocity>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<damping>0.5</damping>
|
||||
<friction>0</friction>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name='lbr_iiwa_link_5'>
|
||||
<pose frame=''>0 -0 0.9645 0 -0 -3.14159</pose>
|
||||
<inertial>
|
||||
<pose frame=''>0.0001 0.021 0.076 0 -0 0</pose>
|
||||
<mass>1.7</mass>
|
||||
<inertia>
|
||||
<ixx>0.02</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.018</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.005</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='lbr_iiwa_link_5_collision'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>meshes/link_5.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name='lbr_iiwa_link_5_visual'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>meshes/link_5.obj</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.5 0.7 1.0 1.0</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name='J4' type='revolute'>
|
||||
<child>lbr_iiwa_link_5</child>
|
||||
<parent>lbr_iiwa_link_4</parent>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<lower>-2.96706</lower>
|
||||
<upper>2.96706</upper>
|
||||
<effort>300</effort>
|
||||
<velocity>10</velocity>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<damping>0.5</damping>
|
||||
<friction>0</friction>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name='lbr_iiwa_link_6'>
|
||||
<pose frame=''>0 0 1.18 1.5708 -0 -3.14159</pose>
|
||||
<inertial>
|
||||
<pose frame=''>0 0.0006 0.0004 0 -0 0</pose>
|
||||
<mass>1.8</mass>
|
||||
<inertia>
|
||||
<ixx>0.005</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.0036</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.0047</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='lbr_iiwa_link_6_collision'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>meshes/link_6.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name='lbr_iiwa_link_6_visual'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>meshes/link_6.obj</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>1.0 0.423529411765 0.0392156862745 1.0</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name='J5' type='revolute'>
|
||||
<child>lbr_iiwa_link_6</child>
|
||||
<parent>lbr_iiwa_link_5</parent>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<lower>-2.0944</lower>
|
||||
<upper>2.0944</upper>
|
||||
<effort>300</effort>
|
||||
<velocity>10</velocity>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<damping>0.5</damping>
|
||||
<friction>0</friction>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name='lbr_iiwa_link_7'>
|
||||
<pose frame=''>0 0 1.261 0 0 0</pose>
|
||||
<inertial>
|
||||
<pose frame=''>0 0 0.02 0 -0 0</pose>
|
||||
<mass>0.3</mass>
|
||||
<inertia>
|
||||
<ixx>0.001</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.001</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.001</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='lbr_iiwa_link_7_collision'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>meshes/link_7.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name='lbr_iiwa_link_7_visual'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>meshes/link_7.obj</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name='J6' type='revolute'>
|
||||
<child>lbr_iiwa_link_7</child>
|
||||
<parent>lbr_iiwa_link_6</parent>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<lower>-3.05433</lower>
|
||||
<upper>3.05433</upper>
|
||||
<effort>300</effort>
|
||||
<velocity>10</velocity>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<damping>0.5</damping>
|
||||
<friction>0</friction>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<!-- Attach the base of the gripper to the end of the arm -->
|
||||
<joint name='gripper_to_arm' type='continuous'>
|
||||
<parent>lbr_iiwa_link_7</parent>
|
||||
<child>base_link</child>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<dynamics>
|
||||
<damping>0.5</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 1.305 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>
|
||||
<box>
|
||||
<size>0.05 0.05 0.1 </size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name='base_left_finger_joint' type='revolute'>
|
||||
<parent>base_link</parent>
|
||||
<child>left_finger</child>
|
||||
<axis>
|
||||
<xyz>0 1 0</xyz>
|
||||
<limit>
|
||||
<lower>-10.4</lower>
|
||||
<upper>10.01</upper>
|
||||
<effort>100</effort>
|
||||
<velocity>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_finger'>
|
||||
<pose frame=''>0 0.024 1.35 0 -0.05 0</pose>
|
||||
<inertial>
|
||||
<pose frame=''>0 0 0.04 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='left_finger_visual'>
|
||||
<pose frame=''>0 0 0.04 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.01 0.01 0.08</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name='left_finger_base_joint' type='fixed'>
|
||||
<parent>left_finger</parent>
|
||||
<child>left_finger_base</child>
|
||||
</joint>
|
||||
<link name='left_finger_base'>
|
||||
<pose frame=''>-0.005 0.024 1.43 0 -0.3 0</pose>
|
||||
<inertial>
|
||||
<pose frame=''>-0.003 0 0.04 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='left_finger_base_visual'>
|
||||
<pose frame=''>0 0 0 0 0 0 </pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1 </scale>
|
||||
<uri>meshes/finger_base_left.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
<collision name='left_finger_base_collision'>
|
||||
<pose frame=''>0 0 0 0 0 0 </pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1 </scale>
|
||||
<uri>meshes/finger_base_left.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name='left_base_tip_joint' type='revolute'>
|
||||
<parent>left_finger_base</parent>
|
||||
<child>left_finger_tip</child>
|
||||
<axis>
|
||||
<xyz>0 1 0</xyz>
|
||||
<limit>
|
||||
<lower>-10.1</lower>
|
||||
<upper>10.3</upper>
|
||||
<effort>0</effort>
|
||||
<velocity>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_finger_tip'>
|
||||
<contact>
|
||||
<lateral_friction>0.8</lateral_friction>
|
||||
<spinning_friction>1.0</spinning_friction>
|
||||
</contact>
|
||||
<pose frame=''>-0.02 0.024 1.49 0 0.2 0</pose>
|
||||
<inertial>
|
||||
<pose frame=''>-0.005 0 0.026 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='left_finger_tip_visual'>
|
||||
<pose frame=''>0 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1 </scale>
|
||||
<uri>meshes/finger_tip_left.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
<collision name='left_finger_tip_collision'>
|
||||
<pose frame=''>0 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1 </scale>
|
||||
<uri>meshes/finger_tip_left.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name='base_right_finger_joint' type='revolute'>
|
||||
<parent>base_link</parent>
|
||||
<child>right_finger</child>
|
||||
<axis>
|
||||
<xyz>0 1 0</xyz>
|
||||
<limit>
|
||||
<lower>-10.01</lower>
|
||||
<upper>10.4</upper>
|
||||
<effort>100</effort>
|
||||
<velocity>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='right_finger'>
|
||||
<pose frame=''>0 0.024 1.35 0 0.05 0</pose>
|
||||
<inertial>
|
||||
<pose frame=''>0 0 0.04 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='right_finger_visual'>
|
||||
<pose frame=''>0 0 0.04 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.01 0.01 0.08</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name='right_finger_base_joint' type='fixed'>
|
||||
<parent>right_finger</parent>
|
||||
<child>right_finger_base</child>
|
||||
</joint>
|
||||
<link name='right_finger_base'>
|
||||
<pose frame=''>0.005 0.024 1.43 0 0.3 0</pose>
|
||||
<inertial>
|
||||
<pose frame=''>0.003 0 0.04 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='right_finger_base_visual'>
|
||||
<pose frame=''>0 0 0 0 0 0 </pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1 </scale>
|
||||
<uri>meshes/finger_base_right.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
<collision name='right_finger_base_collision'>
|
||||
<pose frame=''>0 0 0 0 0 0 </pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1 </scale>
|
||||
<uri>meshes/finger_base_right.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name='right_base_tip_joint' type='revolute'>
|
||||
<parent>right_finger_base</parent>
|
||||
<child>right_finger_tip</child>
|
||||
<axis>
|
||||
<xyz>0 1 0</xyz>
|
||||
<limit>
|
||||
<lower>-10.3</lower>
|
||||
<upper>10.1</upper>
|
||||
<effort>0</effort>
|
||||
<velocity>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='right_finger_tip'>
|
||||
<contact>
|
||||
<lateral_friction>0.8</lateral_friction>
|
||||
<spinning_friction>1.0</spinning_friction>
|
||||
</contact>
|
||||
<pose frame=''>0.02 0.024 1.49 0 -0.2 0</pose>
|
||||
<inertial>
|
||||
<pose frame=''>0.005 0 0.026 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='right_finger_visual'>
|
||||
<pose frame=''>0 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1 </scale>
|
||||
<uri>meshes/finger_tip_right.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 0 0 1</ambient>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
<collision name='right_finger_tip_collision'>
|
||||
<pose frame=''>0 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1 </scale>
|
||||
<uri>meshes/finger_tip_right.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</model>
|
||||
</world>
|
||||
</sdf>
|
||||
@ -10,4 +10,4 @@ Ke 0.000000 0.000000 0.000000
|
||||
Ni 1.000000
|
||||
d 1.000000
|
||||
illum 2
|
||||
#map_Kd tray.jpg
|
||||
map_Kd tray.jpg
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
# Blender v2.77 (sub 0) OBJ File: ''
|
||||
# www.blender.org
|
||||
mtllib tray_textured5.mtl
|
||||
mtllib tray_textured4.mtl
|
||||
o edge_2_Cube
|
||||
v 0.593683 0.625721 0.255175
|
||||
v 0.406317 0.459389 0.004580
|
||||
|
||||
@ -50,8 +50,8 @@ trailDuration = 15
|
||||
logId1 = p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,"LOG0001.txt",[0,1,2])
|
||||
logId2 = p.startStateLogging(p.STATE_LOGGING_CONTACT_POINTS,"LOG0002.txt",bodyUniqueIdA=2)
|
||||
|
||||
for i in xrange(5):
|
||||
print "Body %d's name is %s." % (i, p.getBodyInfo(i)[1])
|
||||
for i in range(5):
|
||||
print ("Body %d's name is %s." % (i, p.getBodyInfo(i)[1]))
|
||||
|
||||
while 1:
|
||||
if (useRealTimeSimulation):
|
||||
|
||||
@ -9,34 +9,59 @@ class Kuka:
|
||||
self.urdfRootPath = urdfRootPath
|
||||
self.timeStep = timeStep
|
||||
self.reset()
|
||||
self.maxForce = 100
|
||||
|
||||
self.maxForce = 90
|
||||
self.fingerAForce = 12
|
||||
self.fingerBForce = 10
|
||||
self.fingerTipForce = 3
|
||||
|
||||
self.useInverseKinematics = 1
|
||||
self.useSimulation = 1
|
||||
self.useNullSpace = 1
|
||||
self.useOrientation = 1
|
||||
self.kukaEndEffectorIndex = 6
|
||||
#lower limits for null space
|
||||
self.ll=[-.967,-2 ,-2.96,0.19,-2.96,-2.09,-3.05]
|
||||
#upper limits for null space
|
||||
self.ul=[.967,2 ,2.96,2.29,2.96,2.09,3.05]
|
||||
#joint ranges for null space
|
||||
self.jr=[5.8,4,5.8,4,5.8,4,6]
|
||||
#restposes for null space
|
||||
self.rp=[0,0,0,0.5*math.pi,0,-math.pi*0.5*0.66,0]
|
||||
#joint damping coefficents
|
||||
self.jd=[0.1,0.1,0.1,0.1,0.1,0.1,0.1]
|
||||
|
||||
def reset(self):
|
||||
|
||||
objects = p.loadSDF("kuka_iiwa/kuka_with_gripper.sdf")
|
||||
objects = p.loadSDF("kuka_iiwa/kuka_with_gripper2.sdf")
|
||||
self.kukaUid = objects[0]
|
||||
for i in range (p.getNumJoints(self.kukaUid)):
|
||||
print(p.getJointInfo(self.kukaUid,i))
|
||||
p.resetBasePositionAndOrientation(self.kukaUid,[-0.100000,0.000000,0.070000],[0.000000,0.000000,0.000000,1.000000])
|
||||
self.jointPositions=[ -0.196884, 0.857586, -0.023543, -1.664977, 0.030403, 0.624786, -0.232294, 0.000000, -0.296450, 0.000000, 0.100002, 0.284399, 0.000000, -0.099999 ]
|
||||
for jointIndex in range (p.getNumJoints(self.kukaUid)):
|
||||
self.numJoints = p.getNumJoints(self.kukaUid)
|
||||
for jointIndex in range (self.numJoints):
|
||||
p.resetJointState(self.kukaUid,jointIndex,self.jointPositions[jointIndex])
|
||||
|
||||
self.trayUid = p.loadURDF("tray/tray.urdf", 0.640000,0.075000,-0.190000,0.000000,0.000000,1.000000,0.000000)
|
||||
self.blockUid =p.loadURDF("block.urdf", 0.604746,0.080626,-0.180069,0.000050,-0.000859,-0.824149,0.566372)
|
||||
|
||||
p.loadURDF("table/table.urdf", 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
|
||||
|
||||
self.motorNames = []
|
||||
self.motorIndices = []
|
||||
numJoints = p.getNumJoints(self.kukaUid)
|
||||
for i in range (numJoints):
|
||||
|
||||
for i in range (self.numJoints):
|
||||
jointInfo = p.getJointInfo(self.kukaUid,i)
|
||||
qIndex = jointInfo[3]
|
||||
if qIndex > -1:
|
||||
print("motorname")
|
||||
print(jointInfo[1])
|
||||
#print("motorname")
|
||||
#print(jointInfo[1])
|
||||
self.motorNames.append(str(jointInfo[1]))
|
||||
self.motorIndices.append(i)
|
||||
|
||||
def getActionDimension(self):
|
||||
return len(self.motorIndices)
|
||||
if (self.useInverseKinematics):
|
||||
return len(self.motorIndices)
|
||||
return 6 #position x,y,z and roll/pitch/yaw euler angles of end effector
|
||||
|
||||
def getObservationDimension(self):
|
||||
return len(self.getObservation())
|
||||
@ -51,8 +76,48 @@ class Kuka:
|
||||
return observation
|
||||
|
||||
def applyAction(self, motorCommands):
|
||||
#print ("self.numJoints")
|
||||
#print (self.numJoints)
|
||||
if (self.useInverseKinematics):
|
||||
pos = [motorCommands[0],motorCommands[1],motorCommands[2]]
|
||||
yaw = motorCommands[3]
|
||||
fingerAngle = motorCommands[4]
|
||||
#roll = motorCommands[5]
|
||||
orn = p.getQuaternionFromEuler([0,-math.pi,0]) # -math.pi,yaw])
|
||||
if (self.useNullSpace==1):
|
||||
if (self.useOrientation==1):
|
||||
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,orn,self.ll,self.ul,self.jr,self.rp)
|
||||
else:
|
||||
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,lowerLimits=self.ll, upperLimits=self.ul, jointRanges=self.jr, restPoses=self.rp)
|
||||
else:
|
||||
if (self.useOrientation==1):
|
||||
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,orn,jointDamping=self.jd)
|
||||
else:
|
||||
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos)
|
||||
|
||||
for action in range (len(motorCommands)):
|
||||
motor = self.motorIndices[action]
|
||||
p.setJointMotorControl2(self.kukaUid,motor,p.POSITION_CONTROL,targetPosition=motorCommands[action],force=self.maxForce)
|
||||
|
||||
#print("jointPoses")
|
||||
#print(jointPoses)
|
||||
#print("self.kukaEndEffectorIndex")
|
||||
#print(self.kukaEndEffectorIndex)
|
||||
if (self.useSimulation):
|
||||
for i in range (self.kukaEndEffectorIndex+1):
|
||||
#print(i)
|
||||
p.setJointMotorControl2(bodyIndex=self.kukaUid,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=self.maxForce,positionGain=0.03,velocityGain=1)
|
||||
else:
|
||||
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
|
||||
for i in range (self.numJoints):
|
||||
p.resetJointState(self.kukaUid,i,jointPoses[i])
|
||||
#fingers
|
||||
p.setJointMotorControl2(self.kukaUid,7,p.POSITION_CONTROL,targetPosition=yaw,force=self.maxForce)
|
||||
p.setJointMotorControl2(self.kukaUid,8,p.POSITION_CONTROL,targetPosition=-fingerAngle,force=self.fingerAForce)
|
||||
p.setJointMotorControl2(self.kukaUid,11,p.POSITION_CONTROL,targetPosition=fingerAngle,force=self.fingerBForce)
|
||||
|
||||
p.setJointMotorControl2(self.kukaUid,10,p.POSITION_CONTROL,targetPosition=0,force=self.fingerTipForce)
|
||||
p.setJointMotorControl2(self.kukaUid,13,p.POSITION_CONTROL,targetPosition=0,force=self.fingerTipForce)
|
||||
|
||||
|
||||
else:
|
||||
for action in range (len(motorCommands)):
|
||||
motor = self.motorIndices[action]
|
||||
p.setJointMotorControl2(self.kukaUid,motor,p.POSITION_CONTROL,targetPosition=motorCommands[action],force=self.maxForce)
|
||||
|
||||
|
||||
@ -20,7 +20,7 @@ class KukaGymEnv(gym.Env):
|
||||
isEnableSelfCollision=True,
|
||||
renders=True):
|
||||
print("init")
|
||||
self._timeStep = 0.01
|
||||
self._timeStep = 1./240.
|
||||
self._urdfRoot = urdfRoot
|
||||
self._actionRepeat = actionRepeat
|
||||
self._isEnableSelfCollision = isEnableSelfCollision
|
||||
@ -45,7 +45,7 @@ class KukaGymEnv(gym.Env):
|
||||
|
||||
def _reset(self):
|
||||
p.resetSimulation()
|
||||
#p.setPhysicsEngineParameter(numSolverIterations=300)
|
||||
p.setPhysicsEngineParameter(numSolverIterations=150)
|
||||
p.setTimeStep(self._timeStep)
|
||||
p.loadURDF("%splane.urdf" % self._urdfRoot,[0,0,-1])
|
||||
|
||||
|
||||
@ -1,14 +1,17 @@
|
||||
|
||||
from envs.bullet.kukaGymEnv import KukaGymEnv
|
||||
print ("hello")
|
||||
import time
|
||||
|
||||
|
||||
environment = KukaGymEnv(renders=True)
|
||||
|
||||
|
||||
motorsIds=[]
|
||||
for i in range (len(environment._kuka.motorNames)):
|
||||
motor = environment._kuka.motorNames[i]
|
||||
motorJointIndex = environment._kuka.motorIndices[i]
|
||||
motorsIds.append(environment._p.addUserDebugParameter(motor,-3,3,environment._kuka.jointPositions[i]))
|
||||
motorsIds.append(environment._p.addUserDebugParameter("posX",0.4,0.75,0.537))
|
||||
motorsIds.append(environment._p.addUserDebugParameter("posY",-.22,.3,0.0))
|
||||
motorsIds.append(environment._p.addUserDebugParameter("posZ",0.1,1,0.2))
|
||||
motorsIds.append(environment._p.addUserDebugParameter("yaw",-3.14,3.14,0))
|
||||
motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3))
|
||||
|
||||
while (True):
|
||||
|
||||
|
||||
23
examples/pybullet/gym/kukaJointSpaceGymEnvTest.py
Normal file
23
examples/pybullet/gym/kukaJointSpaceGymEnvTest.py
Normal file
@ -0,0 +1,23 @@
|
||||
|
||||
from envs.bullet.kukaGymEnv import KukaGymEnv
|
||||
import time
|
||||
|
||||
|
||||
environment = KukaGymEnv(renders=True)
|
||||
environment._kuka.useInverseKinematics=0
|
||||
|
||||
motorsIds=[]
|
||||
for i in range (len(environment._kuka.motorNames)):
|
||||
motor = environment._kuka.motorNames[i]
|
||||
motorJointIndex = environment._kuka.motorIndices[i]
|
||||
motorsIds.append(environment._p.addUserDebugParameter(motor,-3,3,environment._kuka.jointPositions[i]))
|
||||
|
||||
while (True):
|
||||
|
||||
action=[]
|
||||
for motorId in motorsIds:
|
||||
action.append(environment._p.readUserDebugParameter(motorId))
|
||||
|
||||
state, reward, done, info = environment.step(action)
|
||||
obs = environment.getExtendedObservation()
|
||||
time.sleep(0.01)
|
||||
2
setup.py
2
setup.py
@ -419,7 +419,7 @@ else:
|
||||
|
||||
setup(
|
||||
name = 'pybullet',
|
||||
version='1.1.6',
|
||||
version='1.1.7',
|
||||
description='Official Python Interface for the Bullet Physics SDK Robotics Simulator',
|
||||
long_description='pybullet is an easy to use Python module for physics simulation, robotics and machine learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
|
||||
url='https://github.com/bulletphysics/bullet3',
|
||||
|
||||
Loading…
Reference in New Issue
Block a user