diff --git a/python/.README.md.swp b/python/.README.md.swp
deleted file mode 100644
index a6c83f7581c07662841f1005cf20654e377c8946..0000000000000000000000000000000000000000
Binary files a/python/.README.md.swp and /dev/null differ
diff --git a/python/examples/.drawing_from_input_drawing.py.swp b/python/examples/.drawing_from_input_drawing.py.swp
index 777277a1dd36d736b472c96862d313480e100de7..cb6dfeb6f1e8d357a11c2c00ba7b286da2049046 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 f1b661034f4fc7cf04a45b13386bcc2e09905469..14e04c89b2a5debe9193adf70e6bb001950289de 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -31,7 +31,7 @@ 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
+from ur_simple_control.clik.clik_point_to_point import getClikController
#######################################################################
# arguments #
@@ -56,8 +56,10 @@ def get_args():
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)
+# 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, \
@@ -122,28 +124,26 @@ def get_args():
# control loop #
#######################################################################
-def controlLoopWriting(robot, controller, i):
+# feedforward velocity, feedback position and force for impedance
+def controller():
+ pass
+
+def controlLoopWriting(robot, dmp, controller, i):
# dmp
- dmp.step(dt)
+ dmp.step(robot.dt)
# temporal coupling
- tau = dmp.tau + tc.update(dmp, dt) * dt
+ tau = dmp.tau + tc.update(dmp, robot.dt) * robot.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
+ q = robot.getQ()
# TODO: document the mathematics with one-line comments
- wrench = np.array(rtde_receive.getActualTCPForce())
+ 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
- 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
@@ -179,7 +179,7 @@ def controlLoopWriting(robot, controller, i):
if __name__ == "__main__":
args = parser.get_args()
- controller = getController(args)
+ clik_controller = getController(args)
robot = RobotManager(args)
plot_dict = {
'qs' : np.zeros((args.max_iterations, 6)),
@@ -202,8 +202,17 @@ if __name__ == "__main__":
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
+ # test the interplay between this and the speed slider
+ v_max_ndarray = np.ones(6) * robot.max_qd
+ # test the interplay between this and the speed slider
+ # args.acceleration is the actual maximum you're using
+ a_max_ndarray = np.ones(6) * args.acceleration
tc = TCVelAccConstrained(gamma_nominal, gamma_a, v_max, a_max, eps_tc)
+
+ # 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
+ # 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)
diff --git a/python/ur_simple_control/.managers.py.swp b/python/ur_simple_control/.managers.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..a43d7d58d3891f18e11ae16742e2ed16edc12893
Binary files /dev/null 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 f448d0aca693a2d3a204c1725ca5bc5a7dd673cc..57c33bd5da6b047cba444428a0e241de82cfc793 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 a4600275c410d48344cbac8580a9f6c49ac35a2e..54614006e1a9cffd01466c1c0095577a98815eae 100644
--- a/python/ur_simple_control/clik/clik_point_to_point.py
+++ b/python/ur_simple_control/clik/clik_point_to_point.py
@@ -3,7 +3,7 @@ import numpy as np
import copy
import argparse
from functools import partial
-from ur_simple_control.util.boilerplate_wrapper import ControlLoopManager, RobotManager
+from ur_simple_control.managers import ControlLoopManager, RobotManager
"""
Every imaginable magic number, flag and setting is put in here.
@@ -44,7 +44,7 @@ def get_args():
parser.add_argument('--tikhonov-damp', type=float, \
help="damping scalar in tiknohov regularization", default=1e-2)
# TODO add the rest
- parser.add_argument('--controller', type=str, \
+ parser.add_argument('--clik-controller', type=str, \
help="select which click algorithm you want", \
default='dampedPseudoinverse', choices=['dampedPseudoinverse', 'jacobianTranspose'])
# maybe you want to scale the control signal
@@ -76,10 +76,10 @@ if they have extra stuff, just map it in the beginning with partial
NOTE: this could be changed to something else if it proves inappropriate later
TODO: write out other algorithms
"""
-def getController(args):
- if args.controller == "dampedPseudoinverse":
+def getClikController(args):
+ if args.clik_controller == "dampedPseudoinverse":
return partial(dampedPseudoinverse, args.tikhonov_damp)
- if args.controller == "jacobianTranspose":
+ if args.clik_controller == "jacobianTranspose":
return jacobianTranspose
# TODO implement and add in the rest
#if controller_name == "invKinmQPSingAvoidE_kI":
@@ -102,7 +102,7 @@ def getController(args):
# modularity yo
-def controlLoopClik(robot, controller, i):
+def controlLoopClik(robot, clik_controller, i):
breakFlag = False
# know where you are, i.e. do forward kinematics
q = robot.getQ()
@@ -117,7 +117,7 @@ def controlLoopClik(robot, controller, i):
J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
# compute the joint velocities.
# just plug and play different ones
- qd = controller(J, err_vector)
+ qd = clik_controller(J, err_vector)
robot.sendQd(qd)
# we do the printing here because controlLoopManager should be general.
# and these prints are click specific (although i'm not 100% happy with the arrangement)
@@ -134,7 +134,7 @@ if __name__ == "__main__":
args = get_args()
robot = RobotManager(args)
Mgoal = robot.defineGoalPoint()
- controller = getController(args)
- controlLoop = partial(controlLoopClik, robot, controller)
+ clik_controller = getClikController(args)
+ controlLoop = partial(controlLoopClik, robot, clik_controller)
loop_manager = ControlLoopManager(robot, controlLoop, args)
loop_manager.run()
diff --git a/python/ur_simple_control/boilerplate_wrapper.py b/python/ur_simple_control/managers.py
similarity index 86%
rename from python/ur_simple_control/boilerplate_wrapper.py
rename to python/ur_simple_control/managers.py
index 87dce863f6e4e27557d3ba32d721ddeaac19f45f..e9b7f5882d22cb4d1433f049926ff7f411436e1b 100644
--- a/python/ur_simple_control/boilerplate_wrapper.py
+++ b/python/ur_simple_control/managers.py
@@ -15,6 +15,38 @@ from ur_simple_control.util.get_model import get_model
from ur_simple_control.util.robotiq_gripper import RobotiqGripper
import argparse
+"""
+general notes
+---------------
+The first design principle of this library is to minimize the time needed
+to go from a control algorithm on paper to the same control algorithm
+running on the real robot. The second design principle is to have
+the code as simple as possible. In particular, this pertains to avoiding
+overly complex abstractions and having the code as concise as possible.
+The user is expected to read and understand the entire codebase because
+changes will have to accomodate it to their specific project.
+Target users are control engineers.
+The final design choices are made to accommodate these sometimes opposing goals
+to the best of the author's ability.
+
+This file contains a robot manager and a control loop manager.
+The point of these managers is to handle:
+ - boiler plate code around the control loop which is always the same
+ - have all the various parameters neatly organized and in one central location
+ - hide the annoying if-elses of different APIs required
+ for the real robot and various simulations with single commands
+ that just do exactly what you want them to do
+
+current state
+-------------
+Everything is UR specific.
+
+long term vision
+-------------------
+Cut out the robot-specific parts out of the manager classes,
+and create child classes for particular robots.
+"""
+
"""
ControlLoopManager
-------------------
@@ -154,7 +186,6 @@ class RobotManager:
self.rtde_receive = RTDEReceiveInterface("127.0.0.1")
self.rtde_io = RTDEIOInterface("127.0.0.1")
-
# ur specific magic numbers
self.n_joints = 6
# last joint because pinocchio adds base frame as 0th joint.
@@ -167,7 +198,7 @@ class RobotManager:
# and i'm not clipping it, you're fixing it
assert args.acceleration <= 1.7 and args.acceleration > 0.0
# we're not saying it's qdd because it isn't directly (apparently)
- # TODO: check that
+ # TODO: check that again
self.acceleration = args.acceleration
if not args.pinocchio_only:
self.rtde_io.setSpeedSlider(args.speed_slider)
@@ -175,6 +206,7 @@ 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
@@ -212,6 +244,22 @@ class RobotManager:
return self.q
return q
+ """
+ getWrench
+ -----
+ different things need to be send depending on whether you're running a simulation,
+ you're on a real robot, you're running some new simulator bla bla. this is handled
+ here because this things depend on the arguments which are manager here (hence the
+ class name RobotManager)
+ """
+ def getWrench(self):
+ if not self.pinocchio_only:
+ wrench = np.array(self.rtde_receive.getActualTCPForce())
+ else:
+ raise NotImplementedError("Don't have time to implement this right now.")
+
+ return wrench
+
"""
sendQd
-----
@@ -235,6 +283,7 @@ class RobotManager:
self.q = pin.integrate(self.model, self.q, qd * self.dt)
+
"""
defineGoalPoint
------------------