Merge pull request #1189 from erwincoumans/master

tune kuka grasp gym env
This commit is contained in:
erwincoumans
2017-06-15 14:28:30 -07:00
committed by GitHub
13 changed files with 1248 additions and 84 deletions

View File

@@ -3,10 +3,7 @@
<link name="block_2_base_link">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<inertia_scaling value="3.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>
<spinning_friction value=".001"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>

View File

@@ -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>

View File

@@ -0,0 +1,857 @@
<?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>1.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>
</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>0.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>
<collision name='base_link_collision'>
<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>
</collision>
</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.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_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>
<collision name='left_finger_collision'>
<pose frame=''>0 0 0.04 0 0 0</pose>
<geometry>
<box>
<size>0.01 0.01 0.08</size>
</box>
</geometry>
</collision>
</link>
<joint name='left_finger_base_joint' type='fixed'>
<parent>left_finger</parent>
<child>left_finger_base</child>
</joint>
<link name='left_finger_base'>
<contact>
<lateral_friction>0.8</lateral_friction>
<spinning_friction>.1</spinning_friction>
</contact>
<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</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'>
<contact>
<lateral_friction>0.8</lateral_friction>
<spinning_friction>.1</spinning_friction>
</contact>
<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.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.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>
<collision name='right_finger_collision'>
<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>
</collision>
</link>
<joint name='right_finger_base_joint' type='fixed'>
<parent>right_finger</parent>
<child>right_finger_base</child>
</joint>
<link name='right_finger_base'>
<contact>
<lateral_friction>0.8</lateral_friction>
<spinning_friction>.1</spinning_friction>
</contact>
<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</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>

View File

@@ -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

View File

@@ -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

View File

@@ -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):

View File

@@ -0,0 +1,26 @@
import gym
from envs.bullet.kukaGymEnv import KukaGymEnv
from baselines import deepq
def main():
env = KukaGymEnv(renders=True)
act = deepq.load("kuka_model.pkl")
print(act)
while True:
obs, done = env.reset(), False
print("===================================")
print("obs")
print(obs)
episode_rew = 0
while not done:
env.render()
obs, rew, done, _ = env.step(act(obs[None])[0])
episode_rew += rew
print("Episode reward", episode_rew)
if __name__ == '__main__':
main()

View File

@@ -8,43 +8,71 @@ class Kuka:
def __init__(self, urdfRootPath='', timeStep=0.01):
self.urdfRootPath = urdfRootPath
self.timeStep = timeStep
self.reset()
self.maxForce = 100
def reset(self):
objects = p.loadSDF("kuka_iiwa/kuka_with_gripper.sdf")
self.maxForce = 200.
self.fingerAForce = 6
self.fingerBForce = 5.5
self.fingerTipForce = 6
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]
self.reset()
def reset(self):
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.jointPositions=[ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200 ]
self.numJoints = p.getNumJoints(self.kukaUid)
for jointIndex in range (self.numJoints):
p.resetJointState(self.kukaUid,jointIndex,self.jointPositions[jointIndex])
p.setJointMotorControl2(self.kukaUid,jointIndex,p.POSITION_CONTROL,targetPosition=self.jointPositions[jointIndex],force=self.maxForce)
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)
self.endEffectorPos = [0.537,0.0,0.5]
self.endEffectorAngle = 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())
def getObservation(self):
observation = []
pos,orn=p.getBasePositionAndOrientation(self.blockUid)
state = p.getLinkState(self.kukaUid,self.kukaEndEffectorIndex)
pos = state[0]
orn = state[1]
observation.extend(list(pos))
observation.extend(list(orn))
@@ -52,7 +80,81 @@ class Kuka:
def applyAction(self, motorCommands):
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 ("self.numJoints")
#print (self.numJoints)
if (self.useInverseKinematics):
dx = motorCommands[0]
dy = motorCommands[1]
dz = motorCommands[2]
da = motorCommands[3]
fingerAngle = motorCommands[4]
state = p.getLinkState(self.kukaUid,self.kukaEndEffectorIndex)
actualEndEffectorPos = state[0]
#print("pos[2] (getLinkState(kukaEndEffectorIndex)")
#print(actualEndEffectorPos[2])
self.endEffectorPos[0] = self.endEffectorPos[0]+dx
if (self.endEffectorPos[0]>0.75):
self.endEffectorPos[0]=0.75
if (self.endEffectorPos[0]<0.45):
self.endEffectorPos[0]=0.45
self.endEffectorPos[1] = self.endEffectorPos[1]+dy
if (self.endEffectorPos[1]<-0.22):
self.endEffectorPos[1]=-0.22
if (self.endEffectorPos[1]>0.22):
self.endEffectorPos[1]=0.22
#print ("self.endEffectorPos[2]")
#print (self.endEffectorPos[2])
#print("actualEndEffectorPos[2]")
#print(actualEndEffectorPos[2])
if (dz>0 or actualEndEffectorPos[2]>0.10):
self.endEffectorPos[2] = self.endEffectorPos[2]+dz
if (actualEndEffectorPos[2]<0.10):
self.endEffectorPos[2] = self.endEffectorPos[2]+0.0001
self.endEffectorAngle = self.endEffectorAngle + da
pos = self.endEffectorPos
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)
#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=self.endEffectorAngle,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)

View File

@@ -16,22 +16,24 @@ class KukaGymEnv(gym.Env):
def __init__(self,
urdfRoot="",
actionRepeat=50,
actionRepeat=1,
isEnableSelfCollision=True,
renders=True):
print("init")
self._timeStep = 0.01
self._timeStep = 1./240.
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
self._observation = []
self._envStepCounter = 0
self._renders = renders
self.terminated = 0
self._p = p
if self._renders:
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
#timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "kukaTimings.json")
self._seed()
self.reset()
observationDim = len(self.getExtendedObservation())
@@ -39,23 +41,27 @@ class KukaGymEnv(gym.Env):
#print(observationDim)
observation_high = np.array([np.finfo(np.float32).max] * observationDim)
self.action_space = spaces.Discrete(9)
self.action_space = spaces.Discrete(7)
self.observation_space = spaces.Box(-observation_high, observation_high)
self.viewer = None
def _reset(self):
print("reset")
self.terminated = 0
p.resetSimulation()
#p.setPhysicsEngineParameter(numSolverIterations=300)
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
p.loadURDF("%splane.urdf" % self._urdfRoot,[0,0,-1])
if self._renders:
p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33])
p.loadURDF("table/table.urdf", 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
dist = 5 +2.*random.random()
ang = 2.*3.1415925438*random.random()
ballx = dist * math.sin(ang)
bally = dist * math.cos(ang)
ballz = 1
xpos = 0.5 +0.05*random.random()
ypos = 0 +0.05*random.random()
ang = 3.1415925438*random.random()
orn = p.getQuaternionFromEuler([0,0,ang])
self.blockUid =p.loadURDF("block.urdf", xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3])
p.setGravity(0,0,-10)
self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
@@ -72,9 +78,22 @@ class KukaGymEnv(gym.Env):
def getExtendedObservation(self):
self._observation = self._kuka.getObservation()
pos,orn = p.getBasePositionAndOrientation(self.blockUid)
self._observation.extend(list(pos))
self._observation.extend(list(orn))
return self._observation
def _step(self, action):
dv = 0.002
dx = [0,-dv,dv,0,0,0,0][action]
dy = [0,0,0,-dv,dv,0,0][action]
da = [0,0,0,0,0,-0.1,0.1][action]
f = 0.3
realAction = [dx,dy,-0.002,da,f]
return self.step2( realAction)
def step2(self, action):
self._kuka.applyAction(action)
for i in range(self._actionRepeat):
p.stepSimulation()
@@ -84,8 +103,11 @@ class KukaGymEnv(gym.Env):
if self._termination():
break
self._envStepCounter += 1
reward = self._reward()
#print("self._envStepCounter")
#print(self._envStepCounter)
done = self._termination()
reward = self._reward()
#print("len=%r" % len(self._observation))
return np.array(self._observation), reward, done, {}
@@ -94,10 +116,54 @@ class KukaGymEnv(gym.Env):
return
def _termination(self):
return self._envStepCounter>1000
#print (self._kuka.endEffectorPos[2])
state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
actualEndEffectorPos = state[0]
#print("self._envStepCounter")
#print(self._envStepCounter)
if (self.terminated or self._envStepCounter>1000):
self._observation = self.getExtendedObservation()
return True
if (actualEndEffectorPos[2] <= 0.10):
self.terminated = 1
#print("closing gripper, attempting grasp")
#start grasp and terminate
fingerAngle = 0.3
for i in range (1000):
graspAction = [0,0,0.001,0,fingerAngle]
self._kuka.applyAction(graspAction)
p.stepSimulation()
fingerAngle = fingerAngle-(0.3/100.)
if (fingerAngle<0):
fingerAngle=0
self._observation = self.getExtendedObservation()
return True
return False
def _reward(self):
#todo
reward=0
return reward
#rewards is height of target object
blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid)
closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000)
reward = -1000
numPt = len(closestPoints)
#print(numPt)
if (numPt>0):
#print("reward:")
reward = -closestPoints[0][8]*10
if (blockPos[2] >0.2):
print("grasped a block!!!")
print("self._envStepCounter")
print(self._envStepCounter)
reward = reward+1000
#print("reward")
#print(reward)
return reward

View File

@@ -1,21 +1,32 @@
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):
dv = 0.001
motorsIds.append(environment._p.addUserDebugParameter("posX",-dv,dv,0))
motorsIds.append(environment._p.addUserDebugParameter("posY",-dv,dv,0))
motorsIds.append(environment._p.addUserDebugParameter("posZ",-dv,dv,-dv))
motorsIds.append(environment._p.addUserDebugParameter("yaw",-dv,dv,0))
motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3))
done = False
while (not done):
action=[]
for motorId in motorsIds:
action.append(environment._p.readUserDebugParameter(motorId))
state, reward, done, info = environment.step(action)
state, reward, done, info = environment.step2(action)
obs = environment.getExtendedObservation()

View 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)

View File

@@ -0,0 +1,40 @@
import gym
from envs.bullet.kukaGymEnv import KukaGymEnv
from baselines import deepq
import datetime
def callback(lcl, glb):
# stop training if reward exceeds 199
total = sum(lcl['episode_rewards'][-101:-1]) / 100
totalt = lcl['t']
#print("totalt")
#print(totalt)
is_solved = totalt > 2000 and total >= 10
return is_solved
def main():
env = KukaGymEnv(renders=False)
model = deepq.models.mlp([64])
act = deepq.learn(
env,
q_func=model,
lr=1e-3,
max_timesteps=10000000,
buffer_size=50000,
exploration_fraction=0.1,
exploration_final_eps=0.02,
print_freq=10,
callback=callback
)
print("Saving model to kuka_model.pkl")
act.save("kuka_model.pkl")
if __name__ == '__main__':
main()

View File

@@ -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',