diff --git a/python/UNKNOWN.egg-info/PKG-INFO b/python/UNKNOWN.egg-info/PKG-INFO
deleted file mode 100644
index aed30b263857a903b62772da3b28efe924654cb5..0000000000000000000000000000000000000000
--- a/python/UNKNOWN.egg-info/PKG-INFO
+++ /dev/null
@@ -1,11 +0,0 @@
-Metadata-Version: 2.1
-Name: UNKNOWN
-Version: 0.0.0
-Summary: UNKNOWN
-Home-page: UNKNOWN
-License: UNKNOWN
-Platform: UNKNOWN
-License-File: LICENSE.txt
-
-UNKNOWN
-
diff --git a/python/UNKNOWN.egg-info/SOURCES.txt b/python/UNKNOWN.egg-info/SOURCES.txt
deleted file mode 100644
index 4130086e9a697c122668e00de1aade3de571722c..0000000000000000000000000000000000000000
--- a/python/UNKNOWN.egg-info/SOURCES.txt
+++ /dev/null
@@ -1,8 +0,0 @@
-LICENSE.txt
-README.md
-pyproject.toml
-setup.cfg
-UNKNOWN.egg-info/PKG-INFO
-UNKNOWN.egg-info/SOURCES.txt
-UNKNOWN.egg-info/dependency_links.txt
-UNKNOWN.egg-info/top_level.txt
\ No newline at end of file
diff --git a/python/UNKNOWN.egg-info/dependency_links.txt b/python/UNKNOWN.egg-info/dependency_links.txt
deleted file mode 100644
index 8b137891791fe96927ad78e64b0aad7bded08bdc..0000000000000000000000000000000000000000
--- a/python/UNKNOWN.egg-info/dependency_links.txt
+++ /dev/null
@@ -1 +0,0 @@
-
diff --git a/python/UNKNOWN.egg-info/top_level.txt b/python/UNKNOWN.egg-info/top_level.txt
deleted file mode 100644
index 8b137891791fe96927ad78e64b0aad7bded08bdc..0000000000000000000000000000000000000000
--- a/python/UNKNOWN.egg-info/top_level.txt
+++ /dev/null
@@ -1 +0,0 @@
-
diff --git a/python/pyproject.toml b/python/alternative_packaging_BROKEN_atm/pyproject.toml
similarity index 100%
rename from python/pyproject.toml
rename to python/alternative_packaging_BROKEN_atm/pyproject.toml
diff --git a/python/alternative_packaging_BROKEN_atm/setup.cfg b/python/alternative_packaging_BROKEN_atm/setup.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/python/convenience_tool_box/.measuring_stick.py.swp b/python/convenience_tool_box/.measuring_stick.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..565f0d5acdeb8db8100b7857fee177e575e0ce70
Binary files /dev/null and b/python/convenience_tool_box/.measuring_stick.py.swp differ
diff --git a/python/ur_simple_control.egg-info/PKG-INFO b/python/ur_simple_control.egg-info/PKG-INFO
deleted file mode 100644
index 5f07aec0f6e65949b1decf64933d9c84e110f2be..0000000000000000000000000000000000000000
--- a/python/ur_simple_control.egg-info/PKG-INFO
+++ /dev/null
@@ -1,39 +0,0 @@
-Metadata-Version: 2.1
-Name: ur-simple-control
-Version: 0.0.1
-Summary: Collection of control algorithms for the UR5e arm based on the ur_rtde interface for communication and pinocchio for calculations.
-Author-email: Marko Guberina <marko.guberina@control.lth.se>
-Maintainer-email: Marko Guberina <marko.guberina@control.lth.se>
-License: Do whatever you want at your own risk.
-
-Project-URL: Homepage, https://gitlab.control.lth.se/marko-g/ur_simple_control
-Project-URL: Documentation, https://gitlab.control.lth.se/marko-g/ur_simple_control
-Project-URL: Repository, https://gitlab.control.lth.se/marko-g/ur_simple_control
-Project-URL: Bug Tracker, https://gitlab.control.lth.se/marko-g/ur_simple_control
-Project-URL: Changelog, https://gitlab.control.lth.se/marko-g/ur_simple_control
-Keywords: manipulator,control algorithm
-Description-Content-Type: text/markdown
-License-File: LICENSE.txt
-Requires-Dist: ur_rtde
-
-# installation
-------------
-first you MUST update pip, otherwise it won't work:
-python3 -m pip install --upgrade pip setuptools wheel build
-then install with
-pip install --user -e .
-from this directory. now the package is editable, which there's a chance you'd want
-
-# description
----------
-- organized as a library called ur_simple_control, made into a python package. the hope is that if this
- is done well enough, you could mix-and-match different components
- and just have them work as intended. on the other hand,
- the code should still be simple enough to afford the quickest possible prototyping,
- which is the point of having it all in python anyway
-- initial python solution is age-old code which needs to be remapped into the
- libraries used here. it will sit here until everything it offers has been translated.
-
-# runnable things
----------------
-are in the examples folder
diff --git a/python/ur_simple_control.egg-info/SOURCES.txt b/python/ur_simple_control.egg-info/SOURCES.txt
deleted file mode 100644
index c8df78094ede90aec439c4d5d41d82172472d827..0000000000000000000000000000000000000000
--- a/python/ur_simple_control.egg-info/SOURCES.txt
+++ /dev/null
@@ -1,20 +0,0 @@
-LICENSE.txt
-README.md
-pyproject.toml
-setup.py
-ur_simple_control/__init__.py
-ur_simple_control.egg-info/PKG-INFO
-ur_simple_control.egg-info/SOURCES.txt
-ur_simple_control.egg-info/dependency_links.txt
-ur_simple_control.egg-info/requires.txt
-ur_simple_control.egg-info/top_level.txt
-ur_simple_control/clik/__init__.py
-ur_simple_control/clik/clik.py
-ur_simple_control/dmp/__init__.py
-ur_simple_control/dmp/notes.md
-ur_simple_control/dmp/robotiq_gripper.py
-ur_simple_control/dmp/run_dmp.py
-ur_simple_control/util/__init__.py
-ur_simple_control/util/calib_board_hacks.py
-ur_simple_control/util/give_me_the_calibrated_model.py
-ur_simple_control/util/robotiq_gripper.py
\ No newline at end of file
diff --git a/python/ur_simple_control.egg-info/dependency_links.txt b/python/ur_simple_control.egg-info/dependency_links.txt
deleted file mode 100644
index 8b137891791fe96927ad78e64b0aad7bded08bdc..0000000000000000000000000000000000000000
--- a/python/ur_simple_control.egg-info/dependency_links.txt
+++ /dev/null
@@ -1 +0,0 @@
-
diff --git a/python/ur_simple_control.egg-info/requires.txt b/python/ur_simple_control.egg-info/requires.txt
deleted file mode 100644
index e95a92e86f6092929365f59c5d81dd0227e84e5b..0000000000000000000000000000000000000000
--- a/python/ur_simple_control.egg-info/requires.txt
+++ /dev/null
@@ -1 +0,0 @@
-ur_rtde
diff --git a/python/ur_simple_control.egg-info/top_level.txt b/python/ur_simple_control.egg-info/top_level.txt
deleted file mode 100644
index 8b137891791fe96927ad78e64b0aad7bded08bdc..0000000000000000000000000000000000000000
--- a/python/ur_simple_control.egg-info/top_level.txt
+++ /dev/null
@@ -1 +0,0 @@
-
diff --git a/python/ur_simple_control/clik/.clik.py.swp b/python/ur_simple_control/clik/.clik.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..7e2d03a88b0a8fd17bdc30aa40008b6dee547ff7
Binary files /dev/null and b/python/ur_simple_control/clik/.clik.py.swp differ
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 0f5dccae2261d8c97021c2275cf8f2d53cccac1b..7f40d33c8673376eaf071c653fbb2ea878f8f92f 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -1,25 +1,22 @@
import pinocchio as pin
import numpy as np
-import sys
-import os
-from os.path import dirname, join, abspath
-import time
-from pinocchio.visualize import GepettoVisualizer
-#import gepetto.corbaserver
-from rtde_control import RTDEControlInterface as RTDEControl
-from rtde_receive import RTDEReceiveInterface as RTDEReceive
-from rtde_io import RTDEIOInterface
-from robotiq_gripper import RobotiqGripper
-import os
import copy
-import signal
-sys.path.insert(0, '../util')
-from give_me_the_calibrated_model import get_model
import argparse
from functools import partial
-from ur_simple_control.util.boilerplate_wrapper import controlLoopTiming, RobotManager
-
+from ur_simple_control.util.boilerplate_wrapper import ControlLoopManager, RobotManager
+"""
+Every imaginable magic number, flag and setting is put in here.
+This way the rest of the code is clean.
+If you want to know what these various arguments do, just grep
+though the code to find them (but replace '-' with '_' in multi-word arguments).
+All the sane defaults are here, and you can change any number of them as an argument
+when running your program. If you want to change a drastic amount of them, and
+not have to run a super long commands, just copy-paste this function and put your
+own parameters as default ones.
+NOTE: the args object obtained from args = parser.get_args()
+is pushed all around the rest of the code (because it's tidy), so don't change their names.
+"""
def get_args():
parser = argparse.ArgumentParser(description='Run closed loop inverse kinematics \
of various kinds. Make sure you know what the goal is before you run!',
@@ -30,112 +27,111 @@ def get_args():
help="whether you want to just integrate with pinocchio", 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,
+ parser.add_argument('--gripper', action=argparse.BooleanOptionalAction, \
help="whether you're using the gripper", default=False)
- parser.add_argument('--goal-error', type=float,
+ 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,
+ 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 acceleration, positive constant, max 1.7, 0.2 by default. \
- BE CAREFUL WITH THIS.", default=0.4)
- parser.add_argument('--speed-slider', type=float,
+ 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.4)
+ parser.add_argument('--speed-slider', type=float,\
help="cap robot's speed with the speed slider \
to something between 0 and 1, 0.25 by default \
BE CAREFUL WITH THIS.", default=0.25)
- parser.add_argument('--tikhonov-damp', type=float,
- help="damping scalar in tiknohov regularization", default=1e-6)
+ 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, \
+ help="select which click algorithm you want", \
+ default='dampedPseudoinverse', choices=['dampedPseudoinverse', 'jacobianTranspose'])
-# parser.add_argument(
-# '--device', type=str, default='cuda' if torch.cuda.is_available() else 'cpu'
-# )
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')
- if success:
- print("Convergence achieved!")
- else:
- print("Warning: the iterative algorithm has not reached convergence to the desired precision")
-
- print("final error", err_vector.transpose())
return args
-def clik(robot_manager):
- # define goal
- # TODO this should be handled in some better way, but one step at a time
- q = robot.rtde_receive.getActualQ()
- # urdf treats gripper as two prismatic joints,
- # 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
- q.append(0.0)
- q.append(0.0)
- pin.forwardKinematics(robot.model, robot.data, np.array(q))
- Mtool = robot.data.oMi[JOINT_ID]
- print("pos", Mtool)
- #SEerror = pin.SE3(np.zeros((3,3)), np.array([0.0, 0.0, 0.1])
- Mgoal = copy.deepcopy(Mtool)
- Mgoal.translation = Mgoal.translation + np.array([0.0, 0.0, -0.1])
- print("goal", Mgoal)
- success = False
- def controlLoop():
- q = rtde_receive.getActualQ()
- if not SIMULATION:
- gripper_pos = gripper.get_current_position()
- # all 3 are between 0 and 255
- #gripper.move(i % 255, 100, 100)
- # just padding to fill q, which is only needed for forward kinematics
- #q.append(gripper_pos)
- #q.append(gripper_pos)
- q.append(0.0)
- q.append(0.0)
- # pinocchio wants an ndarray
- q = np.array(q)
- pin.forwardKinematics(model, data, q)
- SEerror = data.oMi[JOINT_ID].actInv(Mgoal)
- err_vector = pin.log6(SEerror).vector
- if np.linalg.norm(err_vector) < eps:
- success = True
- print("reached destionation")
- break
- if i >= IT_MAX:
- success = false
- print("FAIL: did not succed in IT_MAX iterations")
- break
- # this does the whole thing unlike the C++ version lel
- J = pin.computeJointJacobian(model, data, q, JOINT_ID)
- #J = J + np.eye(J.shape[0], J.shape[1]) * 10**-4
- # idk what i was thinking here lol
- #v = np.matmul(np.linalg.pinv(J), pin.log(SEerror.inverse() * Mgoal).vector)
- #v = J.T @ err_vector
- #v = np.linalg.pinv(J) @ err_vector
- v = J.T @ np.linalg.inv(J @ J.T + np.eye(J.shape[0], J.shape[0]) * 10**-2) @ err_vector
- v_cmd = v[:6]
- v_cmd = v_cmd * 5
- v_cmd = np.clip(v_cmd, -2, 2)
- if not SIMULATION:
- rtde_control.speedJ(v_cmd, acceleration, dt)
- else:
- q = pin.integrate(model, q, v * dt)
- if not i % 1000:
- print("pos", data.oMi[JOINT_ID])
- print("linear error = ", pin.log6(SEerror).linear)
- print("angular error = ", pin.log6(SEerror).angular)
- print(" error = ", err_vector.transpose())
+#######################################################################
+# controllers #
+#######################################################################
+def dampedPseudoinverse(tiknonov_damp, J, err_vector):
+ qd = J.T @ np.linalg.inv(J @ J.T + np.eye(J.shape[0], J.shape[0]) * tiknonov_damp) @ err_vector
+ return qd
+
+def jacobianTranspose(J, err_vector):
+ qd = J.T @ err_vector
+ return qd
+
+"""
+A string argument is used to select one of these.
+It's a bit ugly, bit totally functional and OK solution.
+we want all of theme to accept the same arguments, i.e. the jacobian and the error vector.
+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":
+ return partial(dampedPseudoinverse, args.tiknonov_damp)
+ if args.controller == "jacobianTranspose":
+ return jacobianTranspose
+ # TODO implement and add in the rest
+ #if controller_name == "invKinmQPSingAvoidE_kI":
+ # return invKinmQPSingAvoidE_kI
+ #if controller_name == "invKinm_PseudoInv":
+ # return invKinm_PseudoInv
+ #if controller_name == "invKinm_PseudoInv_half":
+ # return invKinm_PseudoInv_half
+ #if controller_name == "invKinmQP":
+ # return invKinmQP
+ #if controller_name == "invKinmQPSingAvoidE_kI":
+ # return invKinmQPSingAvoidE_kI
+ #if controller_name == "invKinmQPSingAvoidE_kM":
+ # return invKinmQPSingAvoidE_kM
+ #if controller_name == "invKinmQPSingAvoidManipMax":
+ # return invKinmQPSingAvoidManipMax
+
+ # default
+ return partial(dampedPseudoinverse, args.tiknonov_damp)
- for i in range(robot.args.max_iterations):
- controlLoopTiming(partial(clik, robot))
- if success:
- print("Convergence achieved!")
- else:
- print("Warning: the iterative algorithm has not reached convergence to the desired precision")
+# modularity yo
+def controlLoopClik(robot, controller, i):
+ breakFlag = False
+ # know where you are, i.e. do forward kinematics
+ q = robot.getQ()
+ pin.forwardKinematics(robot.model, robot.data, q)
+ # first check whether we're at the goal
+ SEerror = robot.data.oMi[robot.JOINT_ID].actInv(robot.Mgoal)
+ err_vector = pin.log6(SEerror).vector
+ if np.linalg.norm(err_vector) < eps:
+ print("Convergence achieved, reached destionation!")
+ breakFlag = True
+ # pin.computeJointJacobian is much different than the C++ version lel
+ J = pin.computeJointJacobian(model, data, q, JOINT_ID)
+ # compute the joint velocities.
+ # just plug and play different ones
+ qd = 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)
+ if not i % 1000:
+ print("pos", data.oMi[JOINT_ID])
+ print("linear error = ", pin.log6(SEerror).linear)
+ print("angular error = ", pin.log6(SEerror).angular)
+ print(" error = ", err_vector.transpose())
+ return breakFlag
- print("final error", err_vector.transpose())
- handler(None, None)
if __name__ == "__main__":
args = get_args()
robot = RobotManager(args)
+ Mgoal = robot.defineGoalPoint()
+ controller = getController(args)
+ controlLoop = partial(controlLoopClik, robot, controller)
+ controlLoopManager(controlLoop, args)
+ controlLoopManager.run()
diff --git a/python/ur_simple_control/util/.boilerplate_wrapper.py.swp b/python/ur_simple_control/util/.boilerplate_wrapper.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..d0befeb34512b3259ffe76f42513039b4404548a
Binary files /dev/null and b/python/ur_simple_control/util/.boilerplate_wrapper.py.swp differ
diff --git a/python/ur_simple_control/util/boilerplate_wrapper.py b/python/ur_simple_control/util/boilerplate_wrapper.py
index 9441085d306384b22925a91e6edf85ffc6cd3309..99e0492d741f8c29f28b91c3f3d0308b1ca78474 100644
--- a/python/ur_simple_control/util/boilerplate_wrapper.py
+++ b/python/ur_simple_control/util/boilerplate_wrapper.py
@@ -15,33 +15,95 @@ from ur_simple_control.util.get_model import get_model
from ur_simple_control.util.robotiq_gripper import RobotiqGripper
import argparse
-# slightly fancier programming to get a timing wrapper around the control loop
-def controlLoopTiming(controlLoop):
- start = time.time()
- controlLoop()
- end = time.time()
- diff = end - start
- if dt < diff:
- print("missed deadline by", diff - dt)
- continue
- else:
- time.sleep(dt - diff)
+"""
+controlLoopManager
+-------------------
+Slightly fancier programming 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.
+NOTE: you give this the ready-made control loop.
+if it has arguments, bake them in with functools.partial.
+"""
+class ControlLoopManager(controlLoop, args):
+ def __init__(self, controlLoop, max_iterations):
+ self.max_iterations = args.max_iterations
+ self.controlLoop = controlLoop
-# robotmanager:
-# ---------------
-# - right now it is assumed you're running this on UR5e so some
-# magic numbers are just put to it.
-# this will be extended once there's a need for it.
-# - at this stage it's just a boilerplate reduction class
-# but the idea is to inherit it for more complicated things
-# with many steps, like dmp.
-# or just showe additional things in, this is python after all
-# - you write your controller separately,
-# and then drop it into this - there is a wrapper function you put
-# around the control loop which handles timing so that you
-# actually run at 500Hz and not more.
-# - this is probably not the most new-user friendly solution,
-# but it's designed for fastest idea to implementation 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.
+ # of course have this under the save_plots flag or something similar
+ # save it all in a dictionary of ndarrays and return that
+ # these are all the same (mostly).
+ # if they're not consider inheriting and specializing this or sth similar,
+ # we'll see when we get to it
+
+ """
+ stopHandler
+ -----------
+ # can't have self as first argument, because the
+ # signal handler has to have these 2 arguments (even though they're not used, but
+ # let's be correct if we can)
+ # so it's static, problem solved
+ """
+ @staticmethod
+ def stopHandler(signum, frame):
+ print('sending 100 speedjs full of zeros and exiting')
+ for i in range(100):
+ vel_cmd = np.zeros(6)
+ rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
+ exit()
+
+ """
+ run
+ ---
+ do timing to run at 500Hz.
+ also handle the number of iterations.
+ it's the controlLoop's responsibility to break if it achieved it's goals.
+ this is done via the breakFlag
+ """
+ def run(self):
+ for i in range(self.max_iterations):
+ start = time.time()
+ breakFlag = self.controlLoop(i)
+ if breakFlag:
+ break
+ end = time.time()
+ diff = end - start
+ if dt < diff:
+ print("missed deadline by", diff - dt)
+ continue
+ else:
+ time.sleep(dt - diff)
+ if i < self.max_iterations -1:
+ print("success in", i, "iterations!")
+ else:
+ print("FAIL: did not succed in," max_iterations, "iterations")
+ self.stopHandler(None, None)
+
+"""
+robotmanager:
+---------------
+- right now it is assumed you're running this on UR5e so some
+ magic numbers are just put to it.
+ this will be extended once there's a need for it.
+- at this stage it's just a boilerplate reduction class
+ but the idea is to inherit it for more complicated things
+ with many steps, like dmp.
+ or just showe additional things in, this is python after all
+- you write your controller separately,
+ and then drop it into this - there is a wrapper function you put
+ around the control loop which handles timing so that you
+ actually run at 500Hz and not more.
+- this is probably not the most new-user friendly solution,
+ but it's designed for fastest idea to implementation rate.
+"""
class RobotManager:
# just pass all of the arguments here and store them as is
# so as to minimize the amount of lines.
@@ -66,6 +128,7 @@ class RobotManager:
if args.gripper:
self.gripper = RobotiqGripper()
# initialize and connect the interfaces
+ self.simulation = args.simulation
if not args.simulation:
self.rtde_control = RTDEControlInterface("192.168.1.102")
self.rtde_receive = RTDEReceiveInterface("192.168.1.102")
@@ -78,29 +141,137 @@ class RobotManager:
self.rtde_control = RTDEControlInterface("127.0.0.1")
self.rtde_receive = RTDEReceiveInterface("127.0.0.1")
- # 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)
# ur specific magic numbers
- self.JOINT_ID = 6 # last joint because pinocchio adds base frame as 0th joint
+ self.n_joints = 6
+ # last joint because pinocchio adds base frame as 0th joint.
+ # and since this is unintuitive, we add the other variable too
+ # so that the control designer doesn't need to think about such bs
+ self.JOINT_ID = 6
self.update_rate = 500 #Hz
self.dt = 1 / self.update_rate
+ # you better not give me crazy stuff
+ # 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
self.acceleration = args.acceleration
- rtde_io.setSpeedSlider(args.speed_slider)
+ self.rtde_io.setSpeedSlider(args.speed_slider)
+ self.max_iterations = args.max_iterations
+ # 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
+ self.max_qdd = 1.7
+ # NOTE: i had this thing at 2 in other code
+ self.max_qd = 0.5
+ # maybe you want to scale the control signal
+ self.controller_speed_scaling = 1.0
+
+ """
+ getQ
+ -----
+ urdf treats gripper as two prismatic joints,
+ 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
+ """
+ def getQ(self):
+ q = robot.rtde_receive.getActualQ()
+ if self.args.gripper:
+ 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
+ q.append((self.gripper_pos / 255) * 0.025)
+ q.append((self.gripper_pos / 255) * 0.025)
+ 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
+ q = np.array(q)
+ self.q = q
+ return q
+ """
+ sendQd
+ -----
+ 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 sendQd(self, qd):
+ # we're hiding the extra 2 prismatic joint shenanigans from the control writer
+ # because there you shouldn't need to know this anyway
+ qd_cmd = qd[:6]
+ # maybe you want to scale the control signal
+ qd_cmd = qd * self.controller_speed_scaling
+ # np.clip is ok with bounds being scalar, it does what it should
+ # (but you can also give it an array)
+ qd_cmd = np.clip(qd_cmd, -1 * self.max_qd, self.max_qd)
+ if not self.simulation:
+ # speedj(qd, scalar_lead_axis_acc, hangup_time_on_command)
+ rtde_control.speedJ(qd_cmd, self.acceleration, self.dt)
+ else:
+ # this one takes all 8 elements of qd since we're still in pinocchio
+ self.q = pin.integrate(model, self.q, qd * self.dt)
+
+
+ """
+ defineGoalPoint
+ ------------------
+ --> best way to handle the goal is to tell the user where the gripper is
+ in both UR tcp frame and with pinocchio and have them
+ manually input it when running.
+ this way you force the thinking before the moving,
+ but you also get to view and analyze the information first
+ TODO get the visual thing you did in ivc project with sliders also.
+ it's just text input for now because it's totally usable, just not superb.
+ but also you do want to have both options. obviously you go for the sliders
+ in the case you're visualizing, makes no sense otherwise
+ """
+ def defineGoalPoint(self):
+ q = self.getQ()
+ # define goal
+ pin.forwardKinematics(self.model, self.data, np.array(q))
+ Mtool = self.data.oMi[self.JOINT_ID]
+ if not self.args.visualize:
+ print("You can only specify the translation right now.")
+ print("Here's where the robot is currently. Ensure you know what the base frame is first.")
+
+ print("base frame end-effector pose from pinocchio:\n", \
+ *data.oMi[6].translation.round(4), *pin.rpy.matrixToRpy(data.oMi[6].rotation).round(4))
+ print("UR5e TCP:", *np.array(rtde_receive.getActualTCPPose()).round(4))
+ # remain with the current orientation
+ # TODO: add something, probably rpy for orientation because it's the least number
+ # of numbers you need to type in
+ Mgoal = copy.deepcopy(Mtool)
+ # this is a reasonable way to do it too, maybe implement it later
+ #Mgoal.translation = Mgoal.translation + np.array([0.0, 0.0, -0.1])
+ # do a while loop until this is parsed correctly
+ while True:
+ goal = input("Please enter the target end effector position in the x.x,y.y,z.z format: ")
+ try:
+ e = "ok"
+ goal_list = goal.split(',')
+ for i in range(len(goal_list)):
+ goal_list[i] = float(goal_list[i])
+ except:
+ e = sys.exc_info()
+ print("The input is not in the expected format. Try again.")
+ print(e)
+ if e == "ok":
+ Mgoal.translation = np.array(goal_list)
+ break
+ print("this is goal pose you defined:\n", Mgoal)
+ else:
+ raise NotImplementedError('Paths in the urdf or something else need to \
+ be fixed to use this first. Also need to program in the sliders \
+ and visualizing the goal frame. Sorry. Coming soon.')
+ # NOTE i'm not deepcopying this on purpose
+ # but that might be the preferred thing, we'll see
+ self.Mgoal = Mgoal
+ return Mgoal
- # this does not need to be kept here
- self.q = rtde_receive.getActualQ()
- # can't have self as first argument, because the
- # signal handler has to have these 2 arguments (even though they're not used, but
- # let's be correct if we can)
- # so it's static, problem solved
- @staticmethod
- def stopHandler(signum, frame):
- print('sending 100 speedjs full of zeros and exiting')
- for i in range(100):
- vel_cmd = np.zeros(6)
- rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
- exit()