diff --git a/python/examples/cart_pulling.py b/python/examples/cart_pulling.py
index 9df36a683218250dd71f4f754e4f3340147f1cf2..103355a4a0ad6ca8d169a4e785360471b3cb379b 100644
--- a/python/examples/cart_pulling.py
+++ b/python/examples/cart_pulling.py
@@ -17,6 +17,9 @@ import importlib.util
from ur_simple_control.path_generation.planner import starPlanner, getPlanningArgs, createMap
import yaml
+# TODO:
+# - make reference step size in path_generator an argument here
+# because we use that for path interpolation later on as well
def get_args():
parser = getMinimalArgParser()
@@ -24,6 +27,8 @@ def get_args():
parser = getClikArgs(parser) # literally just for goal error
parser = getPlanningArgs(parser)
parser.add_argument('--handlebar-height', type=float, default=0.5)
+ parser.add_argument('--base-to-handlebar-preferred-distance', type=float, default=0.7, \
+ help="prefered path arclength from mobile base position to handlebar")
argcomplete.autocomplete(parser)
args = parser.parse_args()
# TODO TODO TODO: REMOVE PRESET HACKS
@@ -73,7 +78,8 @@ if __name__ == "__main__":
planning_function = partial(starPlanner, goal)
# TODO: ensure alignment in orientation between planner and actual robot
- path_planner = ProcessManager(args, planning_function, None, 2, None)
+ path_planner = ProcessManager(args, planning_function, robot.q[:2], 3, None)
+ #time.sleep(5)
# clik version
#cartesianPathFollowingWithPlanner(args, robot, path_planner)
# end-effector tracking version
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index 8b2e1e5ebef5450829084854b558d13de09d65bb..c2815be629cbe704bc81afe0a7c810441fb93951 100644
Binary files a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc differ
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 79b63f59d192ad5fe83657c536d722a50d3c3851..c64cd2666bf3ac1fc382e94118990a47ea8f2da4 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -17,7 +17,7 @@ from ur_simple_control.util.get_model import get_model, heron_approximation, her
from collections import deque
from ur_simple_control.visualize.visualize import plotFromDict, realTimePlotter, manipulatorVisualizer
from ur_simple_control.util.logging_utils import LogManager
-from multiprocessing import Process, Queue
+from multiprocessing import Process, Queue, Lock, shared_memory
# argcomplete is an external package which creates tab completion in shell
# argparse is argument parsing from the standard library
import argcomplete, argparse
@@ -277,6 +277,10 @@ class ControlLoopManager:
if self.args.visualize_manipulator:
self.robot_manager.visualizer_manager.sendCommand({"q" : self.robot_manager.q,
"T_w_e" : self.robot_manager.getT_w_e()})
+ if self.robot_manager.robot_name == "heron":
+ T_base = self.robot_manager.data.oMi[1]
+ self.robot_manager.visualizer_manager.sendCommand({"T_base" : T_base})
+
#if self.robot_manager.manipulator_visualizer_queue.qsize() < 5:
# self.robot_manager.updateViz({"q" : self.robot_manager.q,
# "T_w_e" : self.robot_manager.getT_w_e()})
@@ -997,6 +1001,7 @@ class ProcessManager:
# need to create new ones, but i won't optimize in advance
def __init__(self, args, side_function, init_command, comm_direction, init_value=None):
self.args = args
+ self.comm_direction = comm_direction
if comm_direction == 0:
self.command_queue = Queue()
@@ -1011,17 +1016,28 @@ class ProcessManager:
self.data_queue = Queue()
self.side_process = Process(target=side_function,
args=(args, init_command, self.command_queue, self.data_queue,))
+ # both ways, with shared memory for commands
+ if comm_direction == 3:
+ self.data_queue = Queue()
+ # "sending" the command via shared memory
+ shm_name = "command"
+ self.shm = shared_memory.SharedMemory(shm_name, create=True, size=init_command.nbytes)
+ self.shared_command = np.ndarray(init_command.shape, dtype=init_command.dtype, buffer=self.shm.buf)
+ self.shared_command[:] = init_command[:]
+ self.shared_command_lock = Lock()
+ # the process has to create its shared memory
+ self.side_process = Process(target=side_function,
+ args=(args, init_command, shm_name, self.shared_command_lock, self.data_queue,))
if type(side_function) == partial:
self.side_process.name = side_function.func.__name__
else:
self.side_process.name = side_function.__name__ + "_process"
self.lastest_data = init_value
- if self.args.debug_prints:
- print(f"PROCESS_MANAGER: i created the command queue for {self.side_process.name}", self.command_queue)
self.side_process.start()
if self.args.debug_prints:
- print("PROCESS_MANAGER: i am starting {self.side_process.name}")
+ print(f"PROCESS_MANAGER: i am starting {self.side_process.name}")
+
# TODO: enforce that
# the key should be a string containing the command,
@@ -1074,6 +1090,23 @@ class ProcessManager:
#self.command_queue.get_nowait()
self.command_queue.put_nowait(command)
+ # TODO: implement
+ def sendCommandViaSharedMemory(self, command):
+ """
+ sendCommandViaSharedMemory
+ instead of having a queue for the commands, have a shared memory variable.
+ this makes sense if you want to send the latest command only,
+ instead of stacking up old commands in a queue.
+ the locking and unlocking of the shared memory happens here
+ and you don't have to think about it in the control loop nor
+ do you need to pollute half of robotmanager or whatever else
+ to deal with this.
+ """
+ assert type(command) == np.ndarray
+ assert command.shape == self.shared_command.shape
+ self.shared_command_lock.acquire()
+ self.shared_command[:] = command[:]
+ self.shared_command_lock.release()
def getData(self):
if not self.data_queue.empty():
@@ -1081,6 +1114,9 @@ class ProcessManager:
return copy.deepcopy(self.lastest_data)
def terminateProcess(self):
+ if self.comm_direction == 3:
+ self.shm.close()
+ self.shm.unlink()
if self.args.debug_prints:
print(f"i am putting befree in {self.side_process.name}'s command queue to stop it")
self.command_queue.put_nowait("befree")
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index 143ca68edced3eb0942f98f3e38267d5248aa288..078883ce4c0cb408dd1754e43c241ffae5e1dad8 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -221,13 +221,19 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
T_w_e = robot.getT_w_e()
#p = T_w_e.translation[:2]
p = q[:2]
+ print("CTRL:", p)
+ # NOTE: this is the actual position, not what the path suggested
+ # whether this or path reference should be put in is debateable.
+ # this feels more correct to me.
+ save_past_dict['path2D_untimed'] = p
if not (type(path_planner) == types.FunctionType):
- path_planner.sendFreshestCommand({'p' : p})
+ #path_planner.sendFreshestCommand({'p' : p})
+ path_planner.sendCommandViaSharedMemory(p)
# NOTE: it's pointless to recalculate the path every time
# if it's the same 2D path
-
+ # get the path from the base from the current base position onward
if type(path_planner) == types.FunctionType:
pathSE3 = path_planner(T_w_e, i)
else:
@@ -242,19 +248,40 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
if data == "done":
breakFlag = True
- path_pol, path2D_untimed = data
- path2D_untimed = np.array(path2D_untimed).reshape((-1,2))
+ path_pol_base, path2D_untimed_base = data
+ #path2D_untimed_base.insert(0, p[1])
+ #path2D_untimed_base.insert(0, p[0])
+ path2D_untimed_base = np.array(path2D_untimed_base).reshape((-1,2))
# should be precomputed somewhere but this is nowhere near the biggest problem
max_base_v = np.linalg.norm(robot.model.velocityLimit[:2])
# 1) make 6D path out of [[x0,y0],...]
# that represents the center of the cart
- pathSE3 = path2D_to_timed_SE3_base_and_ee(args, path_pol, path2D_untimed, max_base_v)
+ # TODO: USE THIS ONCE IMPLEMENTED
+ #pathSE3 = path2D_to_timed_SE3_base_and_ee(args, path_pol_base, path2D_untimed_base, max_base_v)
+ # TODO: separate out timing construction so that it can be used for the handlebar path as well
+ pathSE3_base = path2D_to_timed_SE3(args, path_pol_base, path2D_untimed_base, max_base_v, 0.0)
# TODO: EVIL AND HAS TO BE REMOVED FROM HERE
if args.visualize_manipulator:
if i % 20 == 0:
- robot.visualizer_manager.sendCommand({"path": pathSE3})
+ robot.visualizer_manager.sendCommand({"path": pathSE3_base})
+ # now find the previous path point of arclength base_to_handlebar_preferred_distance
+ # NOTE: this can be O(1) instead of O(n) but i can't be bothered
+ path_arclength = np.linalg.norm(p - past_data['path2D_untimed'])
+ handlebar_path_index = -1
+ for i in range(-2, -1 * len(past_data['path2D_untimed']), -1):
+ if path_arclength > args.base_to_handlebar_preferred_distance:
+ handlebar_path_index = i
+ break
+ path_arclength += np.linalg.norm(past_data['path2D_untimed'][i - 1] - past_data['path2D_untimed'][i])
+ # i shouldn't need to copy-paste everything but what can you do
+ arra = np.array(past_data['path2D_untimed'])
+ path2D_handlebar = np.vstack((arra[i:], path2D_untimed_base))
+ # now we need to construct timing for this
+
+
+
x0 = np.concatenate([robot.getQ(), robot.getQd()])
solver.problem.x0 = x0
# warmstart solver with previous solution
@@ -263,10 +290,10 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
us_init = list(solver.us[1:]) + [solver.us[-1]]
for i, runningModel in enumerate(solver.problem.runningModels):
- runningModel.differential.costs.costs['gripperPose' + str(i)].cost.residual.reference = pathSE3[i]
+ runningModel.differential.costs.costs['gripperPose' + str(i)].cost.residual.reference = pathSE3_base[i]
# idk if that's necessary
- solver.problem.terminalModel.differential.costs.costs['gripperPose'+str(args.n_knots)].cost.residual.reference = pathSE3[-1]
+ solver.problem.terminalModel.differential.costs.costs['gripperPose'+str(args.n_knots)].cost.residual.reference = pathSE3_base[-1]
solver.solve(xs_init, us_init, args.max_solver_iter)
xs = np.array(solver.xs)
@@ -308,7 +335,12 @@ def BaseAndEEPathFollowingMPC(args, robot, x0, path_planner):
'qs' : np.zeros(robot.model.nq),
'dqs' : np.zeros(robot.model.nv)
}
- save_past_dict = {}
+ # TODO: put ensurance that save_past is not too small for this
+ # or make a specific argument for THIS past-moving-window size
+ # this is the end-effector's reference, so we should initialize that
+ # TODO: verify this initialization actually makes sense
+ T_w_e = robot.getT_w_e()
+ save_past_dict = {'path2D_untimed' : T_w_e.translation[:2]}
loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
loop_manager.run()
diff --git a/python/ur_simple_control/path_generation/planner.py b/python/ur_simple_control/path_generation/planner.py
index 49658b0825e85ed0d25d8844dcd544a0f1b4e285..38eeaf2a88450b36b2a2e53248091e51a1725ea7 100644
--- a/python/ur_simple_control/path_generation/planner.py
+++ b/python/ur_simple_control/path_generation/planner.py
@@ -13,7 +13,7 @@ import pinocchio as pin
import matplotlib.pyplot as plt
import matplotlib.collections as plt_col
-from multiprocessing import Queue
+from multiprocessing import Queue, Lock, shared_memory
def getPlanningArgs(parser):
parser.add_argument('--planning-robot-params-file', type=str,
@@ -462,7 +462,7 @@ def pathVisualizer(x0, goal, map_as_list, positions):
def pathPointFromPathParam(n_pol, path_dim, path_pol, s):
return [np.polyval(path_pol[j*(n_pol+1):(j+1)*(n_pol+1)], s) for j in range(path_dim)]
-def path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v):
+def path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v, path_height):
"""
path2D_to_timed_SE3
---------------------
@@ -514,18 +514,26 @@ def path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v):
total_time = path_length / base_v
# 2) we find the correspondence between s and time
# TODO: read from where it should be, but seba checked that it's 5 for some reason
+ # TODO THIS IS N IN PATH PLANNING, MAKE THIS A SHARED ARGUMENT
max_s = 5
t_to_s = max_s / total_time
# 3) construct the ocp-timed 2D path
+ # TODO MAKE REFERENCE_STEP_SIZE A SHARED ARGUMENT
+ # TODO: we should know max s a priori
+ reference_step_size = 0.5
+ s_vec = np.arange(0, len(path2D_untimed)) / reference_step_size
+
path2D = []
# time is i * args.ocp_dt
for i in range(args.n_knots + 1):
- s = (i * args.ocp_dt) * t_to_s
- path2D.append(pathPointFromPathParam(args.n_pol, args.np, path_pol, s))
+ # what it should be but gets stuck if we're not yet on path
+ #s = (i * args.ocp_dt) * t_to_s
+ # full path
+ s = i * (max_s / args.n_knots)
+ #path2D.append(pathPointFromPathParam(args.n_pol, args.np, path_pol, s))
+ path2D.append(np.array([np.interp(s, s_vec, path2D_untimed[:,0]),
+ np.interp(s, s_vec, path2D_untimed[:,1])]))
path2D = np.array(path2D)
- #print("=" * 15)
- #print(path2D_untimed)
- #print('----------------------------------------')
####################################################
# constructing the SE3 reference from [x,y] path #
@@ -571,10 +579,8 @@ def path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v):
rotation = pin.rpy.rpyToMatrix(0.0, 0.0, thetas[i])
rotation = pin.rpy.rpyToMatrix(np.pi/2, np.pi/2,0.0) @ rotation
#rotation = np.eye(3)
- translation = np.array([path2D[i][0], path2D[i][1],
- args.handlebar_height])
+ translation = np.array([path2D[i][0], path2D[i][1], path_height])
path.append(pin.SE3(rotation, translation))
- #print(path[-1].translation)
return path
@@ -633,7 +639,6 @@ def path2D_to_timed_SE3_base_and_ee(args, path_pol, path2D_untimed, max_base_v):
y_diff = y_i_plus_1 - y_i
y_diff = y_diff.reshape((-1,1))
path_length = np.sum(np.linalg.norm(np.hstack((x_diff, y_diff)), axis=1))
- print(path_length)
total_time = path_length / base_v
# 2) we find the correspondence between s (path parameter) and time
# TODO: read from where max_s should be 5, but seba checked that it's 5 for some reason
@@ -644,14 +649,12 @@ def path2D_to_timed_SE3_base_and_ee(args, path_pol, path2D_untimed, max_base_v):
# TODO: tune this value!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1
# TODO: tune this value!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1
# TODO make this an argument
- base_to_handlebar_prefered_distance = 0.7
# TODO: we need a function that takes time as input and spits
- !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!11
- need to have set interpolation to implement that
+# need to have set interpolation to implement that
# out a path point of distance base_to_handlebar_prefered_distance from time 0.
# (it's the classic integral for curve length)
# cart_to_base_prefered_distance as percentage of path length * size of s (again why the fuck isn't s=1????)
- handlebar_to_base_as_s = (cart_to_base_prefered_distance / path_length) * max_s
+ handlebar_to_base_as_s = (args.base_to_handlebar_preferred_distance / path_length) * max_s
path2D_mobile_base = []
path2D_cart = []
# time is i * args.ocp_dt
@@ -711,13 +714,14 @@ def path2D_to_timed_SE3_base_and_ee(args, path_pol, path2D_untimed, max_base_v):
translation = np.array([path2D_cart[i][0], path2D_cart[i][1],
args.handlebar_height])
path.append(pin.SE3(rotation, translation))
- #print(path[-1].translation)
return path
# function to be put into ProcessManager,
# spitting out path points
-def starPlanner(goal, args, init_cmd, cmd_queue : Queue, data_queue : Queue):
+#def starPlanner(goal, args, init_cmd, cmd_queue : Queue, data_queue : Queue):
+# let's try shared memory
+def starPlanner(goal, args, init_cmd, shm_name, lock : Lock, data_queue : Queue):
"""
starPlanner
------------
@@ -726,6 +730,11 @@ def starPlanner(goal, args, init_cmd, cmd_queue : Queue, data_queue : Queue):
out path points and just the path points.
goal and path are [x,y]
"""
+ # shm stuff
+ shm = shared_memory.SharedMemory(name=shm_name)
+ # dtype is default, but i have to pass it
+ p_shared = np.ndarray((2,), dtype=np.float64, buffer=shm.buf)
+ p = np.zeros(2)
# environment
obstacles, _ = createMap()
@@ -752,8 +761,11 @@ def starPlanner(goal, args, init_cmd, cmd_queue : Queue, data_queue : Queue):
try:
while True:
# has to be a blocking call
- cmd = cmd_queue.get()
- p = cmd['p']
+ #cmd = cmd_queue.get()
+ #p = cmd['p']
+ lock.acquire()
+ p[:] = p_shared[:]
+ lock.release()
if np.linalg.norm(p - goal) < convergence_threshold:
data_queue.put("done")
@@ -765,9 +777,13 @@ def starPlanner(goal, args, init_cmd, cmd_queue : Queue, data_queue : Queue):
r0, rg, rho, obstacles_star = scene_updater.update(p, goal, obstacles)
# compute the path
path_pol, epsilon = path_gen.update(p, r0, rg, rho, obstacles_star)
- data_queue.put((path_pol, path_gen.target_path))
+ # TODO: this is stupid, just used shared memory bro
+ if data_queue.qsize() < 1:
+ data_queue.put((path_pol, path_gen.target_path))
except KeyboardInterrupt:
+ shm.close()
+ shm.unlink()
if args.debug_prints:
print("PLANNER: caught KeyboardInterrupt, i'm out")
diff --git a/python/ur_simple_control/path_generation/tunnel_mpc_params.yaml b/python/ur_simple_control/path_generation/tunnel_mpc_params.yaml
index 1f1ca862f725ed5a3d033dd093dfb0a5238bd9ac..5568e99abeff2546dc3ae8cae9f829f0998bbfc3 100644
--- a/python/ur_simple_control/path_generation/tunnel_mpc_params.yaml
+++ b/python/ur_simple_control/path_generation/tunnel_mpc_params.yaml
@@ -13,7 +13,7 @@ tunnel_mpc:
max_compute_time: 10
crep: 1.
reactivity: 1.
- buffer: 1
+ buffer: 0
# MPC
ce: 100.
@@ -29,6 +29,7 @@ tunnel_mpc:
integration_method: 'euler'
np: 2
n_pol: 10
+ # TODO: test if this is the length of the path
N: 5
dt: 0.2
solver_tol: 1.e-5
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc
index 5f9c8d3e8db06097c501d6e568f9c82e342ef74f..0b9be5219556e6bda86aa8b5060d344b697ac177 100644
Binary files a/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc and b/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc differ
diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc
index 5e23ff210ab7394dfd4a3855298def2a26a4fc03..2095b13bf2b1b332271a3dd4614c22b5bfcd9ccb 100644
Binary files a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc and b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc differ
diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py
index 3c039a0449183cb6c4384a5fa6c869325003d8ba..233245482caf3365bedab4585dc8ee958d6782c8 100644
--- a/python/ur_simple_control/visualize/visualize.py
+++ b/python/ur_simple_control/visualize/visualize.py
@@ -172,6 +172,7 @@ def manipulatorVisualizer(model, collision_model, visual_model, args, cmd, queue
# set shapes we know we'll use
meshcat_shapes.frame(viz.viewer["Mgoal"], opacity=0.5)
meshcat_shapes.frame(viz.viewer["T_w_e"], opacity=0.5)
+ meshcat_shapes.frame(viz.viewer["T_base"], opacity=0.5)
print("MANIPULATORVISUALIZER: FULLY ONLINE")
try:
while True:
@@ -187,6 +188,8 @@ def manipulatorVisualizer(model, collision_model, visual_model, args, cmd, queue
viz.viewer["Mgoal"].set_transform(cmd["Mgoal"].homogeneous)
if key == "T_w_e":
viz.viewer["T_w_e"].set_transform(cmd["T_w_e"].homogeneous)
+ if key == "T_base":
+ viz.viewer["T_base"].set_transform(cmd["T_base"].homogeneous)
if key == "q":
viz.display(cmd["q"])
if key == "point":