diff --git a/python/examples/.drawing_from_input_drawing.py.swp b/python/examples/.drawing_from_input_drawing.py.swp
index cb6dfeb6f1e8d357a11c2c00ba7b286da2049046..424abe01a262c1f14f8d73501a3dc4a74581419a 100644
Binary files a/python/examples/.drawing_from_input_drawing.py.swp and b/python/examples/.drawing_from_input_drawing.py.swp differ
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index 14e04c89b2a5debe9193adf70e6bb001950289de..89d1dabc162841c8e9ecdd9ebe3f013f28209ae1 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -38,6 +38,9 @@ from ur_simple_control.clik.clik_point_to_point import getClikController
#######################################################################
# TODO sort these somehow
def get_args():
+ #######################################################################
+ # generic arguments #
+ #######################################################################
parser = argparse.ArgumentParser(description='Make a drawing on screen,
watch the robot do it on the whiteboard.',
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
@@ -55,23 +58,28 @@ def get_args():
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
-# it's also in the robot manager even though it shouldn't be
- parser.add_argument('--goal-error', type=float, \
- help="the final position error you are happy with. NOTE: not used here", \
- 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('--max-iterations', type=int, \
+ help="maximum allowable iteration number (it runs at 500Hz)", default=100000)
+ #######################################################################
+ # your controller specific arguments #
+ #######################################################################
+ # not applicable here, but leaving it in the case it becomes applicable
+ # it's also in the robot manager even though it shouldn't be
+ parser.add_argument('--past-window-size', type=int, \
+ help="how many timesteps of past data you want to save", default=5)
+ parser.add_argument('--goal-error', type=float, \
+ help="the final position error you are happy with. NOTE: not used here", \
+ default=1e-3)
+ # TODO: test the interaction of this and the overall demo
parser.add_argument('--tikhonov-damp', type=float, \
help="damping scalar in tiknohov regularization.\
This is used when generating the joint trajectory from the drawing.", \
@@ -92,9 +100,6 @@ def get_args():
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", \
@@ -128,77 +133,82 @@ def get_args():
def controller():
pass
-def controlLoopWriting(robot, dmp, controller, i):
- # dmp
- dmp.step(robot.dt)
- # temporal coupling
+# TODO:
+# regarding saving data you have 2 options:
+# 1) explicitely return what you want to save - you can't magically read local variables
+# 2) make controlLoop a class and then save handle the saving behind the scenes -
+# now you these variables are saved in a class so they're not local variables
+# option 1 is clearly more hands-on and thus worse
+# option 2 introduces a third big class and makes everything convoluted.
+# for now, we go for option 1) because it's simpler to implement and deal with.
+# but in the future, implementing 2) should be tried. you really have to try
+# to do it cleanly to see how good/bad it is.
+# in that case you'd most likely want to inherit ControlLoopManager and go from there.
+# you'd still need to specify what you're saving with self.that_variable so no matter
+# there's no running away from that in any case.
+# it's 1) for now, it's the only non-idealy-clean part of this solution, and it's ok really.
+
+# control loop to be passed to ControlLoopManager
+def controlLoopWriting(dmp, tc, controller, robot, i, past_data):
+ breakFlag = False
+ # TODO rename this into something less confusing
+ save_past_dict = {}
+ log_dict = {}
+ dmp.step(robot.dt) # dmp step
+ # temporal coupling step
tau = dmp.tau + tc.update(dmp, robot.dt) * robot.dt
dmp.set_tau(tau)
q = robot.getQ()
- # TODO: document the mathematics with one-line comments
- wrench = robot.getWrench()
- # rolling average
- # TODO: try doing a low-pass filter instead
- # TODO handle past data in some nice way from here or elsewhere?
- 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
- 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))
+ # is actually better, who knows.
+ # also you probably want to do the fusion of that onto tau (got from J.T @ wrench)
+ wrench = robot.getWrench()
+ # deepcopy for good coding practise (and correctness here)
+ save_past_dict['wrench'] = copy.deepcopy(wrench)
+ # rolling average
+ # TODO: try doing a low-pass filter instead
+ # TODO: check where the shape of this is correct (should be, but check)
+ wrench = np.average(np.array(past_data['wrench']), axis=0)
+ pin.forwardKinematics(robot.model, robot.data, q)
+ J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
+ dq = robot.getQd().reshape((6,1))
+ # get joitn
tau = J.T @ wrench
tau = tau[:6].reshape((6,1))
# compute control law:
# - feedforward the velocity and the force reading
# - feedback the position
+ # TODO: don't use vel for qd, it's confusion (yes, that means changing dmp code too)
+ # TODO: put this in a controller function for easy swapping (or don't if you won't swap)
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)
+ robot.sendQd(vel_cmd, acceleration, dt)
+
+ # TODO find a better criterion for stopping
+ if (np.linalg.norm(dmp.vel) < 0.0001) and (i > 5000):
+ breakFlag = True
+ # immediatelly stop if something weird happened (some non-convergence)
if np.isnan(vel_cmd[0]):
- break
- rtde_control.speedJ(vel_cmd, acceleration, dt)
+ breakFlag = True
- # 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,))
+ # log what you said you'd log
+ log_dict['qs'] = q6.reshape((6,))
+ log_dict['dmp_poss'] = dmp.pos.reshape((6,))
+ log_dict['dqs'] = dq.reshape((6,))
+ log_dict['dmp_vels'] = dmp.vel.reshape((6,))
- # TODO do this with something sensible to stop
- if (np.linalg.norm(dmp.vel) < 0.0001) and (i > 5000):
- break
+ return breakFlag, save_past_dict, log_dict
if __name__ == "__main__":
+ #######################################################################
+ # software setup #
+ #######################################################################
args = parser.get_args()
clik_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:
@@ -216,3 +226,30 @@ if __name__ == "__main__":
# ALSO NOTE: to use this you need to change the version inclusions in
# ur_rtde due to a bug there in the current ur_rtde + robot firmware version
# (the bug is it works with the firmware verion, but ur_rtde thinks it doesn't)
+ # here you give what you're saving in the rolling past window
+ # it's initial value.
+ # controlLoopManager will populate the queue with these initial values
+ save_past_dict = {
+ 'wrench' : np.zeros(6),
+ }
+ # here you give it it's initial value
+ log_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)),
+ }
+ controlLoop = partial(controlLoopWriting, dmp, tc, controller, robot)
+ loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_dict)
+ #######################################################################
+ # physical setup #
+ #######################################################################
+ # 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,)))
+
diff --git a/python/ur_simple_control/.managers.py.swp b/python/ur_simple_control/.managers.py.swp
index a43d7d58d3891f18e11ae16742e2ed16edc12893..0b0b860270552b6cb3f46e23f92ce3b1971f0e75 100644
Binary files a/python/ur_simple_control/.managers.py.swp and b/python/ur_simple_control/.managers.py.swp differ
diff --git a/python/ur_simple_control/clik/.clik_point_to_point.py.swp b/python/ur_simple_control/clik/.clik_point_to_point.py.swp
index 57c33bd5da6b047cba444428a0e241de82cfc793..864b03fab06bf205414a942d9890f8385c8f24fb 100644
Binary files a/python/ur_simple_control/clik/.clik_point_to_point.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_point_to_point.py b/python/ur_simple_control/clik/clik_point_to_point.py
index 54614006e1a9cffd01466c1c0095577a98815eae..c98b050fe66b37ed344176c30b5cff8a439ca421 100644
--- a/python/ur_simple_control/clik/clik_point_to_point.py
+++ b/python/ur_simple_control/clik/clik_point_to_point.py
@@ -104,6 +104,7 @@ def getClikController(args):
# modularity yo
def controlLoopClik(robot, clik_controller, i):
breakFlag = False
+ save_past_dict = {}
# know where you are, i.e. do forward kinematics
q = robot.getQ()
pin.forwardKinematics(robot.model, robot.data, q)
@@ -126,7 +127,9 @@ def controlLoopClik(robot, clik_controller, i):
print("linear error = ", pin.log6(SEerror).linear)
print("angular error = ", pin.log6(SEerror).angular)
print(" error = ", err_vector.transpose())
- return breakFlag
+ # we're not saving or loggin here, but need to respect the API,
+ # hence the empty dicts
+ return breakFlag, {}, {}
@@ -136,5 +139,6 @@ if __name__ == "__main__":
Mgoal = robot.defineGoalPoint()
clik_controller = getClikController(args)
controlLoop = partial(controlLoopClik, robot, clik_controller)
- loop_manager = ControlLoopManager(robot, controlLoop, args)
+ # we're not using any past data or logging, hence the empty arguments
+ loop_manager = ControlLoopManager(robot, controlLoop, args, {}, {})
loop_manager.run()
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index e9b7f5882d22cb4d1433f049926ff7f411436e1b..3afdb8111aaf6403249e098b4c57f84af3f70da2 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -1,4 +1,5 @@
# TODO clear out unnecessary imports
+# TODO rename all private variables to start with '_'
import pinocchio as pin
import numpy as np
import time
@@ -14,6 +15,7 @@ import signal
from ur_simple_control.util.get_model import get_model
from ur_simple_control.util.robotiq_gripper import RobotiqGripper
import argparse
+from collections import deque
"""
general notes
@@ -50,7 +52,8 @@ and create child classes for particular robots.
"""
ControlLoopManager
-------------------
-Slightly fancier programming to get a wrapper around the control loop.
+Slightly fancier programming (passing a function as argument and using functools.partial)
+to get a wrapper around the control loop.
In other words, it's the book-keeping around the actual control loop.
It's a class because it keeps non-directly-control-loop-related parameters
like max_iterations, what data to save etc.
@@ -58,7 +61,7 @@ NOTE: you give this the ready-made control loop.
if it has arguments, bake them in with functools.partial.
"""
class ControlLoopManager:
- def __init__(self, robot_manager, controlLoop, args):
+ def __init__(self, robot_manager, controlLoop, args, save_past_dict, log_dict):
self.max_iterations = args.max_iterations
# i need rtde_control do to rtde_control
self.robot_manager = robot_manager
@@ -67,12 +70,6 @@ class ControlLoopManager:
# predefined ur magic numbers
self.update_rate = 500
self.dt = 1 / self.update_rate
-
- # if you just stop it the program with Ctrl-C, it will continue running
- # the last speedj lmao.
- # there's also an actual stop command, but sending 0s works so i'm not changing it
- signal.signal(signal.SIGINT, self.stopHandler)
-
# TODO handle data saving here.
# it can only be handled here because the control loop only cares about the present/
# a small time-window around it.
@@ -82,6 +79,39 @@ class ControlLoopManager:
# if they're not consider inheriting and specializing this or sth similar,
# we'll see when we get to it
+ # it's a dictionaries of deque because deque is the most convenient class to have for removing the first
+ # element and appending a last (it is just a linked list under the hood of course).
+ # it's a dictionary for modularity's sake, because this way you can save whatever you want
+ # and it will just work based on dictionary keys.
+ # it is the user's resposibility to make sure they're providing correct data.
+ # --> but make an assert for the keys at least
+ # in the c++ imlementation, make the user write their own struct or something.
+ # TODO: figure out how to populate it initially
+ # you have the length from the arguments, but you also need the keys.
+ # since those won't be changed, it doesn't make sense to have them as program arguments.
+ # --> make them arguments of this class, there is no better solution
+ self.past_data = {}
+ # save_past_dict has to have the key and 1 example of what you're saving
+ # so that it's type can be inferred (but we're in python so types don't really work).
+ # the good thing is this way you also immediatelly put in the initial values
+ for key in save_past_dict:
+ self.past_data[key] = deque()
+ # immediatelly populate every deque with initial values
+ for i in range(args.past_window_size):
+ # deepcopy just in case, better safe than sorry.
+ # and we can be slow with initialization.
+ self.past_data[key].append(copy.deepcopy(save_past_dict[key]))
+
+ # TODO save and handle plotting data in the exact same fashion.
+ # also write out docs on what's available (and then assert what's defined
+ # to have to be in the list of available things)
+
+ # if you just stop it the program with Ctrl-C, it will continue running
+ # the last speedj lmao.
+ # there's also an actual stop command, but sending 0s works so i'm not changing it
+ signal.signal(signal.SIGINT, self.stopHandler)
+
+
"""
stopHandler
-----------
@@ -109,7 +139,19 @@ class ControlLoopManager:
def run(self):
for i in range(self.max_iterations):
start = time.time()
- breakFlag = self.controlLoop(i)
+ breakFlag, latest_to_save_dict, log_entry_dict = self.controlLoop(i, self.past_data)
+ # update past rolling window
+ # TODO: write an assert assuring the keys are what's been promised
+ # (ideally this is done only once, not every time, so think whether/how that can be avoided)
+ for key in latest_to_save_dict:
+ # remove oldes entry
+ self.past_data[key].popleft()
+ # add new entry
+ self.past_data[key].append(latest_to_save_dict[key])
+
+ # log the data
+
+ # break if done
if breakFlag:
break
end = time.time()
@@ -169,6 +211,16 @@ class RobotManager:
#viz.display(q0)
if self.gripper_flag and not self.pinocchio_only:
self.gripper = RobotiqGripper()
+ # TODO: this is obviously at least a partial redundancy w.r.t. the pinocchio data object
+ # however i have no time to investigate what i get or don't get there
+ # so i'll just leave this be. but fix this!!!! we don't want to duplicate information!!
+ # also TODO: figure out how to best solve the gripper_velocity problem
+ #self.q = np.zeros(6)
+ #self.qd = np.zeros(6)
+ #self.qdd = np.zeros(6)
+ #self.gripper_pos = 0.0
+ #self.gripper_past_pos = 0.0
+ # set q,qd,qdd,griper_pos, gripper_past_pos, gripper_vel to dummy values
# initialize and connect the interfaces
self.simulation = args.simulation
if self.pinocchio_only:
@@ -206,7 +258,6 @@ class RobotManager:
# TODO: these are almost certainly higher
# maybe you want to have them high and cap the rest with speed slider?
# idk really, but it's better to have this low and burried for safety and robot longevity reasons
- # TODO:
self.max_qdd = 1.7
# NOTE: i had this thing at 2 in other code
self.max_qd = 0.5
@@ -221,15 +272,19 @@ class RobotManager:
but they do not affect the overall movement
of the robot, so we add or remove 2 items to the joint list.
also, the gripper is controlled separately so we'd need to do this somehow anyway
+ NOTE: this gripper_past_pos thing is not working atm, but i'll keep it here as a TODO
"""
def getQ(self):
if not self.pinocchio_only:
q = self.rtde_receive.getActualQ()
if self.args.gripper:
+ # TODO: make it work or remove it
+ #self.gripper_past_pos = self.gripper_pos
+ # this is pointless by itself
self.gripper_pos = gripper.get_current_position()
- # the /255 is to get it dimensionless
- # the gap is 5cm
- # thus half the gap is 0.025m and we only do si units here
+ # the /255 is to get it dimensionless.
+ # the gap is 5cm,
+ # thus half the gap is 0.025m (and we only do si units here).
q.append((self.gripper_pos / 255) * 0.025)
q.append((self.gripper_pos / 255) * 0.025)
else:
@@ -244,6 +299,41 @@ class RobotManager:
return self.q
return q
+ """
+ getQd
+ -----
+ same note as getQ.
+ TODO NOTE: atm there's no way to get current gripper velocity.
+ this means you'll probably want to read current positions and then finite-difference
+ to get the velocity.
+ as it stands right now, we'll just pass zeros in because I don't need this ATM
+ """
+ def getQd(self):
+ if not self.pinocchio_only:
+ qd = self.rtde_receive.getActualQd()
+ if self.args.gripper:
+ # TODO: this doesn't work because we're not ensuring stuff is called
+ # at every timestep
+ #self.gripper_vel = (gripper.get_current_position() - self.gripper_pos) / self.dt
+ # so it's just left unused for now - better give nothing than wrong info
+ self.gripper_vel = 0.0
+ # the /255 is to get it dimensionless
+ # the gap is 5cm
+ # thus half the gap is 0.025m and we only do si units here
+ q.append(self.gripper_vel)
+ q.append(self.gripper_vel)
+ else:
+ # just fill it with zeros otherwise
+ q.append(0.0)
+ q.append(0.0)
+ # let's just have both options for getting q, it's just a 8d float list
+ # readability is a somewhat subjective quality after all
+ qd = np.array(qd)
+ self.qd = qd
+ else:
+ return self.qd
+ return qd
+
"""
getWrench
-----
diff --git a/python/ur_simple_control/util/.robotiq_gripper.py.swp b/python/ur_simple_control/util/.robotiq_gripper.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..c8c21fa7784dcb063d1e16b0a39ee21db92da85c
Binary files /dev/null and b/python/ur_simple_control/util/.robotiq_gripper.py.swp differ