diff --git a/python/examples/__pycache__/robotiq_gripper.cpython-310.pyc b/python/examples/__pycache__/robotiq_gripper.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..122384842b716013f5bd36e61282ee1c6cc548bf
Binary files /dev/null and b/python/examples/__pycache__/robotiq_gripper.cpython-310.pyc differ
diff --git a/python/examples/clik.py b/python/examples/clik.py
index 6daff4a7d877228e1de5ef9ea84e8cceb644b836..e537b87c09c66cbadec35f3dfa0277a0e1290692 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik.py
@@ -14,7 +14,7 @@ import os
import copy
import signal
sys.path.insert(0, '../util')
-from give_me_the_calibrated_model import get_model
+from ur_simple_control.util.get_model import get_model
#SIMULATION = True
SIMULATION = False
@@ -28,11 +28,12 @@ PINOCCHIO_ONLY = False
#time.sleep(3)
# load model
-urdf_path_relative = "../robot_descriptions/urdf/ur5e_with_robotiq_hande.urdf"
-urdf_path_absolute = os.path.abspath(urdf_path_relative)
-mesh_dir = "../robot_descriptions/"
-mesh_dir_absolute = os.path.abspath(mesh_dir)
-model, data = get_model(urdf_path_absolute, mesh_dir_absolute)
+VISUALIZE = False
+#urdf_path_relative = "../robot_descriptions/urdf/ur5e_with_robotiq_hande.urdf"
+#urdf_path_absolute = os.path.abspath(urdf_path_relative)
+#mesh_dir = "../robot_descriptions/"
+#mesh_dir_absolute = os.path.abspath(mesh_dir)
+model, collison_model, visual_mode, data = get_model(VISUALIZE)
#load gripper
@@ -66,7 +67,7 @@ Mtool = 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])
+Mgoal.translation = Mgoal.translation + np.array([0.0, 0.0, 0.1])
print("goal", Mgoal)
eps = 1e-3
IT_MAX = 100000
@@ -126,7 +127,6 @@ for i in range(IT_MAX):
#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)
diff --git a/python/ur_simple_control/clik/.clik.py.swp b/python/ur_simple_control/clik/.clik.py.swp
index 9fc488300be91c4a2b8a7a31043e23c54fcc35ba..4b0d127886e9e3b4515a2eba06481e757714f650 100644
Binary files a/python/ur_simple_control/clik/.clik.py.swp 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 6f100662fe064999c940ba28634979336120f585..a4600275c410d48344cbac8580a9f6c49ac35a2e 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -36,17 +36,20 @@ def get_args():
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)
+ TODO: check what this means", default=0.3)
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)
+ 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", 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')
args = parser.parse_args()
if args.gripper and args.simulation:
@@ -133,5 +136,5 @@ if __name__ == "__main__":
Mgoal = robot.defineGoalPoint()
controller = getController(args)
controlLoop = partial(controlLoopClik, robot, controller)
- loop_manager = ControlLoopManager(controlLoop, args)
+ loop_manager = ControlLoopManager(robot, controlLoop, args)
loop_manager.run()
diff --git a/python/ur_simple_control/util/.boilerplate_wrapper.py.swp b/python/ur_simple_control/util/.boilerplate_wrapper.py.swp
index 30b86a09891ea955ae68c488d3e981f4f26b79da..87e036eb49ca0637ee8b9db76e87b12687138579 100644
Binary files a/python/ur_simple_control/util/.boilerplate_wrapper.py.swp and b/python/ur_simple_control/util/.boilerplate_wrapper.py.swp differ
diff --git a/python/ur_simple_control/util/__pycache__/boilerplate_wrapper.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/boilerplate_wrapper.cpython-310.pyc
index 8b3e26aeb2ede980f7ca8364b51c2f398c05e7c0..4496a8bd5d47209e3d9d336dc2a2ca9704df6cc0 100644
Binary files a/python/ur_simple_control/util/__pycache__/boilerplate_wrapper.cpython-310.pyc and b/python/ur_simple_control/util/__pycache__/boilerplate_wrapper.cpython-310.pyc differ
diff --git a/python/ur_simple_control/util/boilerplate_wrapper.py b/python/ur_simple_control/util/boilerplate_wrapper.py
index e93b62d75c830045d473b22d2514402a6acb48e8..87dce863f6e4e27557d3ba32d721ddeaac19f45f 100644
--- a/python/ur_simple_control/util/boilerplate_wrapper.py
+++ b/python/ur_simple_control/util/boilerplate_wrapper.py
@@ -16,7 +16,7 @@ from ur_simple_control.util.robotiq_gripper import RobotiqGripper
import argparse
"""
-controlLoopManager
+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.
@@ -26,8 +26,10 @@ NOTE: you give this the ready-made control loop.
if it has arguments, bake them in with functools.partial.
"""
class ControlLoopManager:
- def __init__(self, controlLoop, args):
+ def __init__(self, robot_manager, controlLoop, args):
self.max_iterations = args.max_iterations
+ # i need rtde_control do to rtde_control
+ self.robot_manager = robot_manager
self.controlLoop = controlLoop
self.args = args
# predefined ur magic numbers
@@ -56,12 +58,12 @@ class ControlLoopManager:
# let's be correct if we can)
# so it's static, problem solved
"""
- @staticmethod
- def stopHandler(signum, frame):
+# @staticmethod
+ def stopHandler(self, 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)
+ self.robot_manager.rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
exit()
"""
@@ -145,8 +147,8 @@ class RobotManager:
self.rtde_io = RTDEIOInterface("192.168.1.102")
if args.gripper:
self.gripper.connect("192.168.1.102", 63352)
- # this is a blocking call
- self.gripper.activate()
+ # this is a blocking call
+ self.gripper.activate()
else:
self.rtde_control = RTDEControlInterface("127.0.0.1")
self.rtde_receive = RTDEReceiveInterface("127.0.0.1")
@@ -163,7 +165,7 @@ class RobotManager:
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
+ 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
@@ -176,8 +178,6 @@ class RobotManager:
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
# error limit
# TODO this is clik specific, put it in a better place
self.goal_error = args.goal_error
@@ -224,8 +224,6 @@ class RobotManager:
# 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)