diff --git a/bricks_etc.py b/bricks_etc.py index 87dc3e5..0f6df4f 100644 --- a/bricks_etc.py +++ b/bricks_etc.py @@ -102,6 +102,97 @@ bricks_end =[{ 'yaw':0 }] +target_positions = [{ + 'num':1, + 'rframe':'t1', + 'x':0.46, + 'y':-0.19, + 'z':0.822, + 'roll':0, + 'pitch':-1.570796, + 'yaw':radians(90) + }, + { + 'num':2, + 'rframe':'a1', + 'x':0, + 'y':0, + 'z':-0.186, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'num':3, + 'rframe':'a1', + 'x':0, + 'y':0, + 'z':-0.372, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'num':4, + 'rframe':'a1', + 'x':0.127, + 'y':0, + 'z':-0.085, + 'roll':0, + 'pitch':1.57, + 'yaw':0 + }, + { + 'num':5, + 'rframe':'a1', + 'x':0.127, + 'y':0, + 'z':-0.287, + 'roll':0, + 'pitch':1.57, + 'yaw':0 + }, + { + 'num':6, + 'rframe':'a1', + 'x':0.254, + 'y':0, + 'z':-0.093, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'num':7, + 'rframe':'a1', + 'x':0.254, + 'y':0, + 'z':-0.287, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'num':8, + 'rframe':'a1', + 'x':0.381, + 'y':0, + 'z':-0.186, + 'roll':0, + 'pitch':1.57, + 'yaw':0 + }, + { + 'num':9, + 'rframe':'a1', + 'x':0.508, + 'y':0, + 'z':-0.186, + 'roll':0, + 'pitch':0, + 'yaw':0 + }] + bricks_start =[{ 'id':'a1', 'rframe':'t1', diff --git a/main.py b/main.py index 0e85aa2..892feef 100755 --- a/main.py +++ b/main.py @@ -55,7 +55,7 @@ class PickAndPlace(object): start_angles = dict(zip(self._joint_names, [0]*7)) self._guarded_move_to_joint_position(start_angles) self.gripper_open() - rospy.sleep(1.0) + rospy.sleep(0.2) print("Running. Ctrl-c to quit") def ik_request(self, pose): @@ -100,11 +100,11 @@ class PickAndPlace(object): def gripper_open(self): self._gripper.open() - rospy.sleep(1.0) + rospy.sleep(0.2) def gripper_close(self): self._gripper.close() - rospy.sleep(1.0) + rospy.sleep(0.2) def _approach(self, pose): approach = copy.deepcopy(pose) @@ -251,8 +251,13 @@ if __name__ == "__main__": - left_test = otc.tf_lookup('a3') - left_pnp.pick(left_test) + left_pick = otc.tf_lookup('a3') + left_pnp.pick(left_pick) + + left_place_pre = otc.tf_lookup('t1') + print() + print(left_place_pre) + print() ####################################HACKING END