From 90bad673ff90d7cf292cd478825f206946c0e251 Mon Sep 17 00:00:00 2001
From: Max Hunt <mh2817@ic.ac.uk>
Date: Mon, 17 Feb 2020 12:36:17 +0000
Subject: [PATCH] Update

---
 brick/model.sdf | 12 ++++++------
 new_main.py     | 16 +++++++++++++++-
 2 files changed, 21 insertions(+), 7 deletions(-)

diff --git a/brick/model.sdf b/brick/model.sdf
index fac3885..61bcc6e 100755
--- a/brick/model.sdf
+++ b/brick/model.sdf
@@ -7,10 +7,10 @@
         <pose frame=''> 0 0 0.0 -0 0</pose>
         <mass>0.1</mass>
         <inertia>
-          <ixx>0.00002</ixx>
+          <ixx>0.000002</ixx>
           <ixy>0</ixy>
           <ixz>0</ixz>
-          <iyy>0.00002</iyy>
+          <iyy>0.000002</iyy>
           <iyz>0</iyz>
           <izz>0</izz>
         </inertia>
@@ -25,8 +25,8 @@
         <surface>
           <friction>
             <ode>
-              <mu>100000</mu>
-              <mu2>10000</mu2>
+              <mu>1000000</mu>
+              <mu2>100000</mu2>
               <fdir1>0 0 1</fdir1>
               <slip1>0.7</slip1>
               <slip2>0.7</slip2>
@@ -34,8 +34,8 @@
           </friction>
           <contact>
             <ode>
-              <kp>10000.000000</kp>
-              <kd>10000.00000</kd>
+              <kp>100000.000000</kp>
+              <kd>1000000.00000</kd>
               <max_vel>20.000000</max_vel>
               <min_depth>0.0001</min_depth>
             </ode>
diff --git a/new_main.py b/new_main.py
index 6ccaece..985c21a 100644
--- a/new_main.py
+++ b/new_main.py
@@ -146,6 +146,20 @@ class PickAndPlace(object):
         # retract to clear object
         self._retract()
 
+    def cust_place(self, pose):
+        # servo above pose
+        approach = copy.deepcopy(pose)
+        # approach with a pose the hover-distance above the requested pose
+        approach.position.z = approach.position.z + 0.1
+        joint_angles = self.ik_request(approach)
+        self._guarded_move_to_joint_position(joint_angles)
+        # servo to pose
+        self._servo_to_pose(pose)
+        # open the gripper
+        self.gripper_open()
+        # retract to clear object
+        self._retract()
+
 
 brick_ids = ['b1','b2','b3','b4','b5','b6','b7','b8','b9']
 
@@ -251,4 +265,4 @@ left_pnp.place(brickstuff[9]['pose'])
 paused = raw_input('Continue?')
 spawn_v_brick()
 left_pnp.pick(brickstuff[0]['pose'])
-left_pnp.place(brickstuff[10]['pose'])
+left_pnp.cust_place(brickstuff[10]['pose'])