diff --git a/python/.setup.py.swp b/python/.README.md.swp
similarity index 84%
rename from python/.setup.py.swp
rename to python/.README.md.swp
index fc1c48cd886b09bab7b4826d832c3761456cf528..a6c83f7581c07662841f1005cf20654e377c8946 100644
Binary files a/python/.setup.py.swp and b/python/.README.md.swp differ
diff --git a/python/README.md b/python/README.md
index 0368c0812f33c799aa5d67c3fa70518f7f033fb9..cc31c69b2903a784f0cbd7ff4069437ea29a1735 100644
--- a/python/README.md
+++ b/python/README.md
@@ -5,6 +5,9 @@
using things on the real robot needs to be tested manually, but simulation in URsimulator should
take care of that because the only difference is the IP address in that case.
point is test what you can.
+- write out types in various functions
+- write appropriate asserts over applicable arguments - you don't want to blow something up
+ due to a misclick while typing out arguments
# installation
------------
diff --git a/python/examples/.drawing_from_input_drawing.py.swp b/python/examples/.drawing_from_input_drawing.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..777277a1dd36d736b472c96862d313480e100de7
Binary files /dev/null and b/python/examples/.drawing_from_input_drawing.py.swp differ
diff --git a/python/examples/clik.py b/python/examples/clik_old.py
similarity index 98%
rename from python/examples/clik.py
rename to python/examples/clik_old.py
index e537b87c09c66cbadec35f3dfa0277a0e1290692..eab2cce9a35b68cfef82a202b7fd3eb861eb3574 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik_old.py
@@ -9,7 +9,7 @@ from pinocchio.visualize import GepettoVisualizer
from rtde_control import RTDEControlInterface as RTDEControl
from rtde_receive import RTDEReceiveInterface as RTDEReceive
from rtde_io import RTDEIOInterface
-from robotiq_gripper import RobotiqGripper
+from ur_simple_control.util.robotiq_gripper import RobotiqGripper
import os
import copy
import signal
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
new file mode 100644
index 0000000000000000000000000000000000000000..f1b661034f4fc7cf04a45b13386bcc2e09905469
--- /dev/null
+++ b/python/examples/drawing_from_input_drawing.py
@@ -0,0 +1,209 @@
+# TODO
+# this is the main file for the drawing your drawings with a dmp with force feedback
+# TODO:
+# 1. clean up these instantiations in a separate util file
+# and just do a line-liner importing -> need to make this a package to get rid
+# of the relative-path imports
+# 2. delete the unnecessary comments
+# 3. figure out how to scale the dmp velocity when creating it -> DONE (arg for tc)
+# 4. parametrize everything, put argparse with defaults in a separate file
+# like in tianshou examples. make magic numbers arguments!
+# also add helpfull error messages based on this, ex. if interfaces couldn't connect
+# with the simulation argument on, write out "check whether you started the simulator"
+# 5. remove all unused code:
+# - for stuff that could work, make a separate file
+# - just remove stuff that's a remnant of past tries
+# 6. put visualization into a separate file for visualization
+# 7. put an actual low-pass filter over the force-torque sensor
+# 8. add some code to pick up the marker from a prespecified location
+# 9. add the (optional) decomposition of force feedback so that
+# you get hybrid motion-force control
+# 10. write documentation as you go along
+# 11. put gripper class in util or something,
+# package it and then have it one place instead of copying it everywhere
+# 12. make this a mainfile, put everything that could be turned into a function elsewhere
+
+from ur_simple_control.dmp.create_dmp import DMP
+from temporal_coupling import NoTC, TCVelAccConstrained
+import pinocchio as pin
+import numpy as np
+import matplotlib.pyplot as plt
+import copy
+from ur_simple_control.util.get_model import get_model
+from ur_simple_control.visualize.visualize import plotFromDict
+from ur_simple_control.clik.clik_point_to_point import getController
+
+#######################################################################
+# arguments #
+#######################################################################
+# TODO sort these somehow
+def get_args():
+ parser = argparse.ArgumentParser(description='Make a drawing on screen,
+ watch the robot do it on the whiteboard.',
+ formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+ # TODO this one won't really work but let's leave it here for the future
+ parser.add_argument('--simulation', action=argparse.BooleanOptionalAction,
+ help="whether you are running the UR simulator. \
+ NOTE: doesn't actually work because it's not a physics simulator", \
+ default=False)
+ parser.add_argument('--pinocchio-only', action=argparse.BooleanOptionalAction,
+ help="whether you want to just integrate with pinocchio.\
+ NOTE: doesn't actually work because it's not a physics simulator", \
+ default=False)
+ parser.add_argument('--visualize', action=argparse.BooleanOptionalAction,
+ help="whether you want to visualize with gepetto, but \
+ NOTE: not implemented yet", default=False)
+ parser.add_argument('--gripper', action=argparse.BooleanOptionalAction, \
+ help="whether you're using the gripper", default=True)
+# not applicable here, but leaving it in the case it becomes applicable
+# parser.add_argument('--goal-error', type=float, \
+# help="the final position error you are happy with", default=1e-3)
+ parser.add_argument('--max-iterations', type=int, \
+ help="maximum allowable iteration number (it runs at 500Hz)", default=100000)
+ parser.add_argument('--acceleration', type=float, \
+ help="robot's joints acceleration. scalar positive constant, \
+ max 1.7, and default 0.4. \
+ BE CAREFUL WITH THIS. the urscript doc says this is 'lead axis acceleration'.\
+ TODO: check what this means", default=0.3)
+ # TODO: test the interaction of this and the overall demo
+ parser.add_argument('--speed-slider', type=float,\
+ help="cap robot's speed with the speed slider \
+ to something between 0 and 1, 0.5 by default \
+ BE CAREFUL WITH THIS.", default=0.5)
+ parser.add_argument('--tikhonov-damp', type=float, \
+ help="damping scalar in tiknohov regularization.\
+ This is used when generating the joint trajectory from the drawing.", \
+ default=1e-2)
+ # TODO add the rest
+ parser.add_argument('--controller', type=str, \
+ help="select which click algorithm you want", \
+ default='dampedPseudoinverse', \
+ choices=['dampedPseudoinverse', 'jacobianTranspose'])
+ # maybe you want to scale the control signal
+ parser.add_argument('--controller-speed-scaling', type=float, \
+ default='1.0', help='not actually_used atm')
+ ############################
+ # dmp specific arguments #
+ ############################
+ parser.add_argument('--temporal-coupling', action=argparse.BooleanOptionalAction, \
+ help="whether you want to use temporal coupling", default=True)
+ parser.add_argument('--kp', type=float, \
+ help="proportial control constant for position errors", \
+ default=2.0)
+ parser.add_argument('--kp', type=float, \
+ help="proportial control constant for position errors", \
+ default=2)
+ parser.add_argument('--tau0', type=float, \
+ help="total time needed for trajectory. if you use temporal coupling,\
+ you can still follow the path even if it's too fast", \
+ default=5)
+ parser.add_argument('--gamma-nominal', type=float, \
+ help="positive constant for tuning temporal coupling: the higher,\
+ the fast the return rate to nominal tau", \
+ default=1.0)
+ parser.add_argument('--gamma-a', type=float, \
+ help="positive constant for tuning temporal coupling, potential term", \
+ default=0.5)
+ parser.add_argument('--eps-tc', type=float, \
+ help="temporal coupling term, should be small", \
+ default=0.001)
+ parser.add_argument('--alpha', type=float, \
+ help="force feedback proportional coefficient", \
+ default=0.003)
+ # TODO add low pass filtering and make it's parameters arguments too
+
+ args = parser.parse_args()
+ if args.gripper and args.simulation:
+ raise NotImplementedError('Did not figure out how to put the gripper in \
+ the simulation yet, sorry :/ . You can have only 1 these flags right now')
+ return args
+
+#######################################################################
+# control loop #
+#######################################################################
+
+def controlLoopWriting(robot, controller, i):
+ # dmp
+ dmp.step(dt)
+ # temporal coupling
+ tau = dmp.tau + tc.update(dmp, dt) * dt
+ dmp.set_tau(tau)
+ q = rtde_receive.getActualQ()
+ # TODO and NOTE the weight, TCP and inertial matrix needs to be set on the robot
+ # you already found an API in rtde_control for this, just put it in initialization
+ # under using/not-using gripper parameters
+ # TODO: document the mathematics with one-line comments
+ wrench = np.array(rtde_receive.getActualTCPForce())
+ # rolling average
+ # TODO: try doing a low-pass filter instead
+ wrench_avg[i % 5] = wrench
+ wrench = np.average(wrench_avg, axis=0)
+ # pinocchio model is 8-dof because it treats the gripper
+ # as two prismatic joints
+ q6 = np.array(copy.deepcopy(q))
+ q.append(0.0)
+ q.append(0.0)
+ q = np.array(q)
+ pin.forwardKinematics(model, data, q)
+ # calculate joint torques based on the force-torque sensor
+ # TODO look into UR code/api for estimating the same
+ # based on currents in the joints.
+ # it's probably worse, but maybe some sensor fusion-type thing
+ # is actually better, who knows
+ J = pin.computeJointJacobian(model, data, q, JOINT_ID)
+ dq = np.array(rtde_receive.getActualQd()).reshape((6,1))
+ tau = J.T @ wrench
+ tau = tau[:6].reshape((6,1))
+ # compute control law:
+ # - feedforward the velocity and the force reading
+ # - feedback the position
+ vel_cmd = dmp.vel + kp * (dmp.pos - q6.reshape((6,1))) - alpha * tau
+ vel_cmd = vel_cmd.reshape((6,))
+ # just in case, clip the velocity to max values
+ vel_cmd = np.clip(vel_cmd, -1 * v_max, v_max)
+ # and immediatelly stop if something weird happened (some non-convergence)
+ if np.isnan(vel_cmd[0]):
+ break
+ rtde_control.speedJ(vel_cmd, acceleration, dt)
+
+ # save values for the plot
+ # TODO cut off at the last time instant?
+ qs[i] = q6.reshape((6,))
+ dmp_poss[i] = dmp.pos.reshape((6,))
+ dqs[i] = dq.reshape((6,))
+ dmp_vels[i] = dmp.vel.reshape((6,))
+
+ # TODO do this with something sensible to stop
+ if (np.linalg.norm(dmp.vel) < 0.0001) and (i > 5000):
+ break
+
+if __name__ == "__main__":
+ args = parser.get_args()
+ controller = getController(args)
+ robot = RobotManager(args)
+ plot_dict = {
+ 'qs' : np.zeros((args.max_iterations, 6)),
+ 'dmp_poss' : np.zeros((args.max_iterations, 6)),
+ 'dqs' : np.zeros((args.max_iterations, 6)),
+ 'dmp_vels' : np.zeros((args.max_iterations, 6)),
+ }
+
+ # load trajectory, create DMP based on it
+ dmp = DMP()
+ # TODO: add marker picking
+ # get up from the board
+ current_pose = rtde_receive.getActualTCPPose()
+ current_pose[2] = current_pose[2] + 0.03
+ rtde_control.moveL(current_pose)
+ # move to initial pose
+ # this is a blocking call
+ rtde_control.moveJ(dmp.pos.reshape((6,)))
+
+ if not args.temporal_coupling:
+ tc = NoTC()
+ else:
+ # TODO learn the math already
+ # TODO test whether this works (it should, but test it)
+ v_max = np.ones(6) * 0.5 * speed_slider
+ a_max = np.ones(6) * 1.7
+ tc = TCVelAccConstrained(gamma_nominal, gamma_a, v_max, a_max, eps_tc)
diff --git a/python/examples/force_mode_api.py b/python/examples/force_mode_api.py
new file mode 100644
index 0000000000000000000000000000000000000000..3bb59da509afc6c174fcaffe8448f959b27c2f58
--- /dev/null
+++ b/python/examples/force_mode_api.py
@@ -0,0 +1,40 @@
+# this is just random code which needs to be put into
+# a concrete example.
+# but the chunks and some comments make sense,
+# so they're kept here for future use
+
+#######################################################################
+# some force_mode from ur api chucks #
+#######################################################################
+
+# ========================================================================
+# TODO: either write a separate file where this is used
+# ---> TODO write a separate file where you're testing this out
+# or just delete it
+task_frame = [0, 0, 0, 0, 0, 0]
+# these are in {0,1} and select which task frame direction compliance is active in
+# just be soft everywhere
+selection_vector = [1, 1, 1, 1, 1, 1]
+# the wrench applied to the environment:
+# position is adjusted to achieve the specified wrench
+# let's pretend speedjs are this and see what happens (idk honestly)
+wrench = [0, 0, 0, 0, 0, 0]
+ftype = 2
+# limits for:
+# - compliant axes: highest tcp velocities allowed on compliant axes
+# - non-compliant axes: maximum tcp position error compared to the program (which prg,
+# and where is this set?)
+# why these values?
+limits = [2, 2, 2, 2, 2, 2]
+wrench_avg = np.zeros((5,6))
+
+# TODO: move to existing force mode API testing file
+# this is very stupind, but it was quick to implement
+vel_cmd8 = list(vel_cmd)
+vel_cmd8.append(0.0)
+vel_cmd8.append(0.0)
+vel_cmd8 = np.array(vel_cmd8)
+vel_tcp = J @ vel_cmd8
+vel_tcp = vel_tcp * 10
+rtde_control.forceMode(task_frame, selection_vector, vel_tcp, ftype, limits)
+# ========================================================================
diff --git a/python/examples/robotiq_gripper.py b/python/examples/robotiq_gripper.py
deleted file mode 100644
index a17ab18b99a15f62dc5dff3b0785589ca33b37ac..0000000000000000000000000000000000000000
--- a/python/examples/robotiq_gripper.py
+++ /dev/null
@@ -1,291 +0,0 @@
-"""Module to control Robotiq's grippers - tested with HAND-E"""
-
-import socket
-import threading
-import time
-from enum import Enum
-from typing import Union, Tuple, OrderedDict
-
-class RobotiqGripper:
- """
- Communicates with the gripper directly, via socket with string commands, leveraging string names for variables.
- """
- # WRITE VARIABLES (CAN ALSO READ)
- ACT = 'ACT' # act : activate (1 while activated, can be reset to clear fault status)
- GTO = 'GTO' # gto : go to (will perform go to with the actions set in pos, for, spe)
- ATR = 'ATR' # atr : auto-release (emergency slow move)
- ADR = 'ADR' # adr : auto-release direction (open(1) or close(0) during auto-release)
- FOR = 'FOR' # for : force (0-255)
- SPE = 'SPE' # spe : speed (0-255)
- POS = 'POS' # pos : position (0-255), 0 = open
- # READ VARIABLES
- STA = 'STA' # status (0 = is reset, 1 = activating, 3 = active)
- PRE = 'PRE' # position request (echo of last commanded position)
- OBJ = 'OBJ' # object detection (0 = moving, 1 = outer grip, 2 = inner grip, 3 = no object at rest)
- FLT = 'FLT' # fault (0=ok, see manual for errors if not zero)
-
- ENCODING = 'UTF-8' # ASCII and UTF-8 both seem to work
-
- class GripperStatus(Enum):
- """Gripper status reported by the gripper. The integer values have to match what the gripper sends."""
- RESET = 0
- ACTIVATING = 1
- # UNUSED = 2 # This value is currently not used by the gripper firmware
- ACTIVE = 3
-
- class ObjectStatus(Enum):
- """Object status reported by the gripper. The integer values have to match what the gripper sends."""
- MOVING = 0
- STOPPED_OUTER_OBJECT = 1
- STOPPED_INNER_OBJECT = 2
- AT_DEST = 3
-
- def __init__(self):
- """Constructor."""
- self.socket = None
- self.command_lock = threading.Lock()
- self._min_position = 0
- self._max_position = 255
- self._min_speed = 0
- self._max_speed = 255
- self._min_force = 0
- self._max_force = 255
-
- def connect(self, hostname: str, port: int, socket_timeout: float = 2.0) -> None:
- """Connects to a gripper at the given address.
- :param hostname: Hostname or ip.
- :param port: Port.
- :param socket_timeout: Timeout for blocking socket operations.
- """
- self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
- self.socket.connect((hostname, port))
- self.socket.settimeout(socket_timeout)
-
- def disconnect(self) -> None:
- """Closes the connection with the gripper."""
- self.socket.close()
-
- def _set_vars(self, var_dict: OrderedDict[str, Union[int, float]]):
- """Sends the appropriate command via socket to set the value of n variables, and waits for its 'ack' response.
- :param var_dict: Dictionary of variables to set (variable_name, value).
- :return: True on successful reception of ack, false if no ack was received, indicating the set may not
- have been effective.
- """
- # construct unique command
- cmd = "SET"
- for variable, value in var_dict.items():
- cmd += f" {variable} {str(value)}"
- cmd += '\n' # new line is required for the command to finish
- # atomic commands send/rcv
- with self.command_lock:
- self.socket.sendall(cmd.encode(self.ENCODING))
- data = self.socket.recv(1024)
- return self._is_ack(data)
-
- def _set_var(self, variable: str, value: Union[int, float]):
- """Sends the appropriate command via socket to set the value of a variable, and waits for its 'ack' response.
- :param variable: Variable to set.
- :param value: Value to set for the variable.
- :return: True on successful reception of ack, false if no ack was received, indicating the set may not
- have been effective.
- """
- return self._set_vars(OrderedDict([(variable, value)]))
-
- def _get_var(self, variable: str):
- """Sends the appropriate command to retrieve the value of a variable from the gripper, blocking until the
- response is received or the socket times out.
- :param variable: Name of the variable to retrieve.
- :return: Value of the variable as integer.
- """
- # atomic commands send/rcv
- with self.command_lock:
- cmd = f"GET {variable}\n"
- self.socket.sendall(cmd.encode(self.ENCODING))
- data = self.socket.recv(1024)
-
- # expect data of the form 'VAR x', where VAR is an echo of the variable name, and X the value
- # note some special variables (like FLT) may send 2 bytes, instead of an integer. We assume integer here
- var_name, value_str = data.decode(self.ENCODING).split()
- if var_name != variable:
- raise ValueError(f"Unexpected response {data} ({data.decode(self.ENCODING)}): does not match '{variable}'")
- value = int(value_str)
- return value
-
- @staticmethod
- def _is_ack(data: str):
- return data == b'ack'
-
- def _reset(self):
- """
- Reset the gripper.
- The following code is executed in the corresponding script function
- def rq_reset(gripper_socket="1"):
- rq_set_var("ACT", 0, gripper_socket)
- rq_set_var("ATR", 0, gripper_socket)
-
- while(not rq_get_var("ACT", 1, gripper_socket) == 0 or not rq_get_var("STA", 1, gripper_socket) == 0):
- rq_set_var("ACT", 0, gripper_socket)
- rq_set_var("ATR", 0, gripper_socket)
- sync()
- end
-
- sleep(0.5)
- end
- """
- self._set_var(self.ACT, 0)
- self._set_var(self.ATR, 0)
- while (not self._get_var(self.ACT) == 0 or not self._get_var(self.STA) == 0):
- self._set_var(self.ACT, 0)
- self._set_var(self.ATR, 0)
- time.sleep(0.5)
-
-
- def activate(self, auto_calibrate: bool = True):
- """Resets the activation flag in the gripper, and sets it back to one, clearing previous fault flags.
- :param auto_calibrate: Whether to calibrate the minimum and maximum positions based on actual motion.
- The following code is executed in the corresponding script function
- def rq_activate(gripper_socket="1"):
- if (not rq_is_gripper_activated(gripper_socket)):
- rq_reset(gripper_socket)
-
- while(not rq_get_var("ACT", 1, gripper_socket) == 0 or not rq_get_var("STA", 1, gripper_socket) == 0):
- rq_reset(gripper_socket)
- sync()
- end
-
- rq_set_var("ACT",1, gripper_socket)
- end
- end
- def rq_activate_and_wait(gripper_socket="1"):
- if (not rq_is_gripper_activated(gripper_socket)):
- rq_activate(gripper_socket)
- sleep(1.0)
-
- while(not rq_get_var("ACT", 1, gripper_socket) == 1 or not rq_get_var("STA", 1, gripper_socket) == 3):
- sleep(0.1)
- end
-
- sleep(0.5)
- end
- end
- """
- if not self.is_active():
- self._reset()
- while (not self._get_var(self.ACT) == 0 or not self._get_var(self.STA) == 0):
- time.sleep(0.01)
-
- self._set_var(self.ACT, 1)
- time.sleep(1.0)
- while (not self._get_var(self.ACT) == 1 or not self._get_var(self.STA) == 3):
- time.sleep(0.01)
-
- # auto-calibrate position range if desired
- if auto_calibrate:
- self.auto_calibrate()
-
- def is_active(self):
- """Returns whether the gripper is active."""
- status = self._get_var(self.STA)
- return RobotiqGripper.GripperStatus(status) == RobotiqGripper.GripperStatus.ACTIVE
-
- def get_min_position(self) -> int:
- """Returns the minimum position the gripper can reach (open position)."""
- return self._min_position
-
- def get_max_position(self) -> int:
- """Returns the maximum position the gripper can reach (closed position)."""
- return self._max_position
-
- def get_open_position(self) -> int:
- """Returns what is considered the open position for gripper (minimum position value)."""
- return self.get_min_position()
-
- def get_closed_position(self) -> int:
- """Returns what is considered the closed position for gripper (maximum position value)."""
- return self.get_max_position()
-
- def is_open(self):
- """Returns whether the current position is considered as being fully open."""
- return self.get_current_position() <= self.get_open_position()
-
- def is_closed(self):
- """Returns whether the current position is considered as being fully closed."""
- return self.get_current_position() >= self.get_closed_position()
-
- def get_current_position(self) -> int:
- """Returns the current position as returned by the physical hardware."""
- return self._get_var(self.POS)
-
- def auto_calibrate(self, log: bool = True) -> None:
- """Attempts to calibrate the open and closed positions, by slowly closing and opening the gripper.
- :param log: Whether to print the results to log.
- """
- # first try to open in case we are holding an object
- (position, status) = self.move_and_wait_for_pos(self.get_open_position(), 64, 1)
- if RobotiqGripper.ObjectStatus(status) != RobotiqGripper.ObjectStatus.AT_DEST:
- raise RuntimeError(f"Calibration failed opening to start: {str(status)}")
-
- # try to close as far as possible, and record the number
- (position, status) = self.move_and_wait_for_pos(self.get_closed_position(), 64, 1)
- if RobotiqGripper.ObjectStatus(status) != RobotiqGripper.ObjectStatus.AT_DEST:
- raise RuntimeError(f"Calibration failed because of an object: {str(status)}")
- assert position <= self._max_position
- self._max_position = position
-
- # try to open as far as possible, and record the number
- (position, status) = self.move_and_wait_for_pos(self.get_open_position(), 64, 1)
- if RobotiqGripper.ObjectStatus(status) != RobotiqGripper.ObjectStatus.AT_DEST:
- raise RuntimeError(f"Calibration failed because of an object: {str(status)}")
- assert position >= self._min_position
- self._min_position = position
-
- if log:
- print(f"Gripper auto-calibrated to [{self.get_min_position()}, {self.get_max_position()}]")
-
- def move(self, position: int, speed: int, force: int) -> Tuple[bool, int]:
- """Sends commands to start moving towards the given position, with the specified speed and force.
- :param position: Position to move to [min_position, max_position]
- :param speed: Speed to move at [min_speed, max_speed]
- :param force: Force to use [min_force, max_force]
- :return: A tuple with a bool indicating whether the action it was successfully sent, and an integer with
- the actual position that was requested, after being adjusted to the min/max calibrated range.
- """
-
- def clip_val(min_val, val, max_val):
- return max(min_val, min(val, max_val))
-
- clip_pos = clip_val(self._min_position, position, self._max_position)
- clip_spe = clip_val(self._min_speed, speed, self._max_speed)
- clip_for = clip_val(self._min_force, force, self._max_force)
-
- # moves to the given position with the given speed and force
- var_dict = OrderedDict([(self.POS, clip_pos), (self.SPE, clip_spe), (self.FOR, clip_for), (self.GTO, 1)])
- return self._set_vars(var_dict), clip_pos
-
- def move_and_wait_for_pos(self, position: int, speed: int, force: int) -> Tuple[int, ObjectStatus]: # noqa
- """Sends commands to start moving towards the given position, with the specified speed and force, and
- then waits for the move to complete.
- :param position: Position to move to [min_position, max_position]
- :param speed: Speed to move at [min_speed, max_speed]
- :param force: Force to use [min_force, max_force]
- :return: A tuple with an integer representing the last position returned by the gripper after it notified
- that the move had completed, a status indicating how the move ended (see ObjectStatus enum for details). Note
- that it is possible that the position was not reached, if an object was detected during motion.
- """
- set_ok, cmd_pos = self.move(position, speed, force)
- if not set_ok:
- raise RuntimeError("Failed to set variables for move.")
-
- # wait until the gripper acknowledges that it will try to go to the requested position
- while self._get_var(self.PRE) != cmd_pos:
- time.sleep(0.001)
-
- # wait until not moving
- cur_obj = self._get_var(self.OBJ)
- while RobotiqGripper.ObjectStatus(cur_obj) == RobotiqGripper.ObjectStatus.MOVING:
- cur_obj = self._get_var(self.OBJ)
-
- # report the actual position and the object status
- final_pos = self._get_var(self.POS)
- final_obj = cur_obj
- return final_pos, RobotiqGripper.ObjectStatus(final_obj)
\ No newline at end of file
diff --git a/python/ur_simple_control/__init__.py b/python/ur_simple_control/__init__.py
index 38c21a51e1c542c6d8e3da9be01d84d24364e066..ce2a563955379d574c4a23691ff14de19b8c2ec6 100644
--- a/python/ur_simple_control/__init__.py
+++ b/python/ur_simple_control/__init__.py
@@ -1,4 +1,4 @@
-from ur_simple_control import clik, dmp, robot_descriptions, util
+from ur_simple_control import clik, dmp, robot_descriptions, util, visualize
__all__ = [
-"clik", "dmp", "robot_descriptions", "util",
+"clik", "dmp", "robot_descriptions", "util", "visualize",
]
diff --git a/python/ur_simple_control/util/boilerplate_wrapper.py b/python/ur_simple_control/boilerplate_wrapper.py
similarity index 100%
rename from python/ur_simple_control/util/boilerplate_wrapper.py
rename to python/ur_simple_control/boilerplate_wrapper.py
diff --git a/python/ur_simple_control/clik/.clik.py.swp b/python/ur_simple_control/clik/.clik.py.swp
deleted file mode 100644
index 4b0d127886e9e3b4515a2eba06481e757714f650..0000000000000000000000000000000000000000
Binary files a/python/ur_simple_control/clik/.clik.py.swp and /dev/null differ
diff --git a/python/ur_simple_control/util/.get_model.py.swp b/python/ur_simple_control/clik/.clik_point_to_point.py.swp
similarity index 51%
rename from python/ur_simple_control/util/.get_model.py.swp
rename to python/ur_simple_control/clik/.clik_point_to_point.py.swp
index bd148d33717e53ec67b40270ae0735a98b999d35..f448d0aca693a2d3a204c1725ca5bc5a7dd673cc 100644
Binary files a/python/ur_simple_control/util/.get_model.py.swp and b/python/ur_simple_control/clik/.clik_point_to_point.py.swp differ
diff --git a/python/ur_simple_control/clik/.clik_trajectory_following.py.swp b/python/ur_simple_control/clik/.clik_trajectory_following.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..80ba7275dae3fbd9f50fa6b4ef4c35cbf0713b9d
Binary files /dev/null and b/python/ur_simple_control/clik/.clik_trajectory_following.py.swp differ
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik_point_to_point.py
similarity index 100%
rename from python/ur_simple_control/clik/clik.py
rename to python/ur_simple_control/clik/clik_point_to_point.py
diff --git a/python/ur_simple_control/dmp/drawing_gen/cliking_the_path.py b/python/ur_simple_control/clik/clik_trajectory_following.py
similarity index 100%
rename from python/ur_simple_control/dmp/drawing_gen/cliking_the_path.py
rename to python/ur_simple_control/clik/clik_trajectory_following.py
diff --git a/python/ur_simple_control/dmp/dmp/create_dmp.py b/python/ur_simple_control/dmp/create_dmp.py
similarity index 100%
rename from python/ur_simple_control/dmp/dmp/create_dmp.py
rename to python/ur_simple_control/dmp/create_dmp.py
diff --git a/python/ur_simple_control/dmp/dmp/temporal_coupling.py b/python/ur_simple_control/dmp/temporal_coupling.py
similarity index 100%
rename from python/ur_simple_control/dmp/dmp/temporal_coupling.py
rename to python/ur_simple_control/dmp/temporal_coupling.py
diff --git a/python/ur_simple_control/util/.boilerplate_wrapper.py.swp b/python/ur_simple_control/util/.boilerplate_wrapper.py.swp
deleted file mode 100644
index 87e036eb49ca0637ee8b9db76e87b12687138579..0000000000000000000000000000000000000000
Binary files a/python/ur_simple_control/util/.boilerplate_wrapper.py.swp and /dev/null differ
diff --git a/python/ur_simple_control/dmp/drawing_gen/draw_path.py b/python/ur_simple_control/util/draw_path.py
similarity index 100%
rename from python/ur_simple_control/dmp/drawing_gen/draw_path.py
rename to python/ur_simple_control/util/draw_path.py
diff --git a/python/ur_simple_control/visualize/__init__.py b/python/ur_simple_control/visualize/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py
new file mode 100644
index 0000000000000000000000000000000000000000..a1cb99723b48825efaf99d7615bf824b08b9e2d3
--- /dev/null
+++ b/python/ur_simple_control/visualize/visualize.py
@@ -0,0 +1,28 @@
+import numpy as np
+import matplotlib.pyplot as plt
+
+# TODO let's try to make this general
+# make plot_data a dictionary.
+# every key is one subplot, and it's value
+# is what you plot
+def plotFromDict(plot_data, args):
+ # TODO: replace with actual time
+ # --> you should be able to extract/construct this from dmp data
+ # or some timing
+ t = np.arange(args.max_iterations)
+ # TODO make the plot number assignment general
+ n_cols = 2
+ n_rows = 2
+ # this is what subplot wants
+ subplot_col_row = str(n_cols) + str(n_rows)
+ ax_dict ={}
+ # TODO: choose a nice color palette
+ colors = plt.cm.jet(np.linspace(0, 1, len(plot_data)))
+ for i, data_key in enumerate(plot_data):
+ ax_dict[data_key] = plt.subplot(subplot_col_row + str(i))
+ for j in range(len(plot_data[data_key])):
+ ax_dict[data_key].plot(t, plot_data[data_key][:,j], color=colors[i], label=day_key)
+ # TODO maybe write a hack for getting the legend to work (all lines are the same color so
+ # we're not using it as intended
+ ax_dict[data_key].legend()
+ plt.show()