From 1b00cb583fa2c95e46190254dc8e6526cd25c5cf Mon Sep 17 00:00:00 2001
From: drMJ <drMJ@users.noreply.github.com>
Date: Mon, 6 Feb 2023 13:37:30 -0800
Subject: [PATCH] added move_and_grasp

---
 examples/relative.py | 28 ----------------------------
 roman/robot.py       | 28 +++++++++++++++-------------
 roman/rq/hand.py     |  5 +++++
 3 files changed, 20 insertions(+), 41 deletions(-)
 delete mode 100644 examples/relative.py

diff --git a/examples/relative.py b/examples/relative.py
deleted file mode 100644
index ef51b96..0000000
--- a/examples/relative.py
+++ /dev/null
@@ -1,28 +0,0 @@
-import os
-rootdir = os.path.dirname(os.path.dirname(__file__))
-os.sys.path.insert(0, rootdir)
-
-# this brings in most of the relevant parts. It requires numpy, scipy and pybullet
-import math
-import random
-from roman import *
-
-def rand(max):
-    return (random.random()-0.5)*max
-
-if __name__ == '__main__':
-    # connect to robot.  
-    robot = connect(use_sim=True)
-    
-    # move the arm in small relative increments
-    t_step = 0.1
-    r_step = 1
-    for i in range(5):
-        robot.move_simple(rand(t_step), rand(t_step), rand(t_step), rand(r_step), max_speed = 0.5)
-
-    # now step instead (sustain the move for a fixed time)
-    for i in range(40):
-        robot.step(rand(t_step), rand(t_step), rand(t_step), rand(r_step), max_speed = 1, max_acc=1)
-
-    robot.disconnect()
-    
diff --git a/roman/robot.py b/roman/robot.py
index 21da81f..2185190 100644
--- a/roman/robot.py
+++ b/roman/robot.py
@@ -87,19 +87,6 @@ class Robot:
         self.__check_connected()
         return self.hand.state.object_detected()
 
-    def move_simple(self, dx, dy, dz, dyaw, max_speed=0.5, max_acc=0.5, timeout=0.2):
-        '''
-        Moves the arm relative to the current position in carthesian coordinates,
-        assuming the gripper is vertical (aligned with the z-axis), pointing down.
-        This version returns after the amount of time specified by dt.
-        '''
-        self.__check_connected()
-        self.arm.read()
-        pose = self.arm.state.tool_pose()
-        pose = Tool.from_xyzrpy(pose.to_xyzrpy() + [dx, dy, dz, 0, 0, dyaw])
-        self.arm.move(pose, max_speed=max_speed, max_acc=max_acc, blocking=False)
-        self.__complete_move(timeout, None)
-
     def get_inverse_kinematics(self, target):
         self.__check_connected()
         return self.arm.get_inverse_kinematics(target)
@@ -170,6 +157,21 @@ class Robot:
                        blocking=False)
         return self.__complete_move(timeout, completion)
 
+    def move_and_grasp(self,
+             arm_target,
+             max_speed=UR_DEFAULT_MAX_SPEED,
+             max_acc=UR_DEFAULT_ACCELERATION,
+             force_limit=None,
+             max_final_speed=0,
+             grasp_target=Position.CLOSED,
+             grasp_speed=255,
+             grasp_force=0,
+             timeout=None,
+             completion=None):
+        self.__check_connected()
+        self.hand.move(position=grasp_target, speed=grasp_speed, force=grasp_force, blocking=False)
+        return self.move(target=arm_target, max_speed=max_speed, max_acc=max_acc, force_limit=force_limit, max_final_speed=max_final_speed, timeout=timeout, completion=completion)
+
     def set_hand_mode(self, mode=GraspMode.BASIC):
         self.__check_connected()
         self.hand.set_mode(mode)
diff --git a/roman/rq/hand.py b/roman/rq/hand.py
index f288557..491e3a4 100644
--- a/roman/rq/hand.py
+++ b/roman/rq/hand.py
@@ -172,6 +172,11 @@ class Hand:
         self.command.make(Command._CMD_KIND_CHANGE, Finger.All, 0, 0, 0, mode)
         self.__execute(True)
 
+    @staticmethod
     def normalize(position):
         return position / 255
 
+    @staticmethod
+    def from_normalized(position):
+        return int(position * 255)
+