diff --git a/examples/graz/clik_client.py b/examples/graz/clik_client.py
index aca40dc8f90f809159bfb3804b72cce1f7e3edeb..079e1a68892b405eeb7853626c2bcfd3ef15f702 100644
--- a/examples/graz/clik_client.py
+++ b/examples/graz/clik_client.py
@@ -41,7 +41,7 @@ def controlLoopClikExternalGoal(
ik_solver,
i,
past_data,
-):
+) -> tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]:
"""
controlLoop
-----------------------------
@@ -61,11 +61,12 @@ def controlLoopClikExternalGoal(
SEerror = T_w_e.actInv(T_goal)
err_vector = pin.log6(SEerror).vector
J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
+ K = np.diag([30] * 3 + [15] * 3)
qd_cmd = (
J.T
@ np.linalg.inv(J @ J.T + np.eye(J.shape[0], J.shape[0]) * args.tikhonov_damp)
- @ (err_vector + data["v"])
+ @ (K @ err_vector + data["v"])
)
if qd_cmd is None:
print("the controller you chose didn't work, using dampedPseudoinverse instead")
diff --git a/python/smc/control/controller_templates/generic_control_loop.py b/python/smc/control/controller_templates/generic_control_loop.py
new file mode 100644
index 0000000000000000000000000000000000000000..f9b5838124dface1e6e89cfac18f9937c36866ce
--- /dev/null
+++ b/python/smc/control/controller_templates/generic_control_loop.py
@@ -0,0 +1,28 @@
+from argparse import Namespace
+from typing import Any, Callable
+import numpy as np
+
+from smc.robots.robotmanager_abstract import AbstractRobotManager
+
+global control_loop_return
+control_loop_return = tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]
+
+
+def GenericControlLoop(
+ X: Any,
+ control_loop: Callable[
+ [Any, Namespace, AbstractRobotManager, dict[str, np.ndarray], float],
+ control_loop_return,
+ ],
+ args: Namespace,
+ robot: AbstractRobotManager,
+ past_data: dict[str, np.ndarray],
+ t: int, # will be float eventually
+) -> control_loop_return:
+ breakFlag = False
+ log_item = {}
+ save_past_item = {}
+
+ control_loop(X, args, robot, past_data, t)
+
+ return breakFlag, log_item, save_past_item
diff --git a/python/smc/control/controller_templates/path_following_template.py b/python/smc/control/controller_templates/path_following_template.py
new file mode 100644
index 0000000000000000000000000000000000000000..4dd851c1f036606f5d57215d1989132a325ca01d
--- /dev/null
+++ b/python/smc/control/controller_templates/path_following_template.py
@@ -0,0 +1,62 @@
+from smc.robots.robotmanager_abstract import AbstractRobotManager
+from smc.multiprocessing.process_manager import ProcessManager
+
+from pinocchio import SE3, log6
+from argparse import Namespace
+from typing import Any, Callable
+import numpy as np
+
+global control_loop_return
+control_loop_return = tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]
+
+
+def PathFollowingFromPlannerControlLoop(
+ path_planner: ProcessManager,
+ get_position: Callable[[AbstractRobotManager], np.ndarray],
+ SOLVER: Callable,
+ control_loop: Callable[
+ [Any, np.ndarray, Namespace, AbstractRobotManager, dict[str, np.ndarray], int],
+ np.ndarray,
+ ],
+ args: Namespace,
+ robot: AbstractRobotManager,
+ past_data: dict[str, np.ndarray],
+ t: int, # will be float eventually
+) -> control_loop_return:
+ """
+ Point2PointControlLoop
+ ---------------
+ generic control loop for point to point motion
+ (handling error to final point etc).
+ """
+ breakFlag = False
+ log_item = {}
+ save_past_item = {}
+
+ p = get_position(robot)
+
+ path_planner.sendCommand(p)
+ data = path_planner.getData()
+
+ if data == "done":
+ breakFlag = True
+
+ if data == "done" or data is None:
+ robot.sendVelocityCommand(np.zeros(robot.model.nv))
+ log_item["qs"] = robot.q
+ log_item["dqs"] = robot.v
+ return breakFlag, save_past_item, log_item
+
+ _, path2D_untimed = data
+ path2D_untimed = np.array(path2D_untimed).reshape((-1, 2))
+
+ v_cmd, log_item_inner = control_loop(
+ SOLVER, path2D_untimed, args, robot, past_data, t
+ )
+ robot.sendVelocityCommand(v_cmd)
+ log_item.update(log_item_inner)
+
+ log_item["qs"] = robot.q
+ log_item["dqs"] = robot.v
+ log_item["dqs_cmd"] = v_cmd.reshape((robot.model.nv,))
+ return breakFlag, save_past_item, log_item
diff --git a/python/smc/control/controller_templates/point_to_point.py b/python/smc/control/controller_templates/point_to_point.py
new file mode 100644
index 0000000000000000000000000000000000000000..1f2a61ee780585294fb1d510474f5fd694b8fb60
--- /dev/null
+++ b/python/smc/control/controller_templates/point_to_point.py
@@ -0,0 +1,46 @@
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+
+from pinocchio import SE3, log6
+from argparse import Namespace
+from typing import Any, Callable
+import numpy as np
+
+global control_loop_return
+control_loop_return = tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]
+
+
+def Point2PointControlLoop(
+ SOLVER: Callable,
+ T_w_goal: SE3,
+ control_loop: Callable[
+ [Any, SE3, Namespace, SingleArmInterface, dict[str, np.ndarray], int],
+ np.ndarray,
+ ],
+ args: Namespace,
+ robot: SingleArmInterface,
+ past_data: dict[str, np.ndarray],
+ t: int, # will be float eventually
+) -> control_loop_return:
+ """
+ Point2PointControlLoop
+ ---------------
+ generic control loop for point to point motion
+ (handling error to final point etc).
+ """
+ breakFlag = False
+ log_item = {}
+ save_past_item = {}
+
+ v_cmd = control_loop(SOLVER, T_w_goal, args, robot, past_data, t)
+ robot.sendVelocityCommand(v_cmd)
+
+ T_w_e = robot.T_w_e
+ SEerror = T_w_e.actInv(T_w_goal)
+ err_vector = log6(SEerror).vector
+ if np.linalg.norm(err_vector) < robot.args.goal_error:
+ breakFlag = True
+ log_item["qs"] = robot.q
+ log_item["err_norm"] = np.linalg.norm(err_vector).reshape((1,))
+ log_item["dqs"] = robot.v
+ log_item["dqs_cmd"] = v_cmd.reshape((robot.model.nv,))
+ return breakFlag, save_past_item, log_item
diff --git a/python/smc/control/optimal_control/croco_mpc_path_following.py b/python/smc/control/optimal_control/croco_mpc_path_following.py
index bd1ec8913f3221895e32b6924751640e5138a172..e6500f3f409034b0ad17062e7dc30b2b2d1d1843 100644
--- a/python/smc/control/optimal_control/croco_mpc_path_following.py
+++ b/python/smc/control/optimal_control/croco_mpc_path_following.py
@@ -301,28 +301,30 @@ def BaseAndEEPathFollowingFromPlannerMPCControlLoop(
log_item["err_norm_ee"] = np.linalg.norm(err_vector_ee).reshape((1,))
log_item["err_norm_base"] = np.linalg.norm(err_vector_base).reshape((1,))
log_item["qs"] = q.reshape((robot.model.nq,))
- #log_item["dqs"] = robot.v.reshape((robot.model.nv,))
+ # log_item["dqs"] = robot.v.reshape((robot.model.nv,))
return breakFlag, save_past_dict, log_item
-def initializePastData(args: Namespace, T_w_e: SE3, p_base: np.ndarray, max_base_v: float) -> np.ndarray:
+def initializePastData(
+ args: Namespace, T_w_e: SE3, p_base: np.ndarray, max_base_v: float
+) -> np.ndarray:
# prepopulate past data to make base and cart be on the same path in the past
# (which didn't actually happen because this just started)
p_ee = T_w_e.translation[:2]
straight_line_path = np.linspace(p_ee, p_base, args.past_window_size)
straight_line_path_timed = path2D_timed(args, straight_line_path, max_base_v)
# time it the same way the base path is timed
-# time_past = np.linspace(
-# 0.0, args.past_window_size * (1 / args.ctrl_freq), args.past_window_size
-# )
-# s = np.linspace(0.0, args.n_knots * args.ocp_dt, args.past_window_size)
-# path2D_handlebar = np.hstack(
-# (
-# np.interp(s, time_past, straight_line_path[:, 0]).reshape((-1, 1)),
-# np.interp(s, time_past, straight_line_path[:, 1]).reshape((-1, 1)),
-# )
-# )
-# return path2D_handlebar
+ # time_past = np.linspace(
+ # 0.0, args.past_window_size * (1 / args.ctrl_freq), args.past_window_size
+ # )
+ # s = np.linspace(0.0, args.n_knots * args.ocp_dt, args.past_window_size)
+ # path2D_handlebar = np.hstack(
+ # (
+ # np.interp(s, time_past, straight_line_path[:, 0]).reshape((-1, 1)),
+ # np.interp(s, time_past, straight_line_path[:, 1]).reshape((-1, 1)),
+ # )
+ # )
+ # return path2D_handlebar
return straight_line_path
@@ -363,7 +365,7 @@ def BaseAndEEPathFollowingMPC(
)
log_item = {
"qs": np.zeros(robot.model.nq),
- # "dqs": np.zeros(robot.model.nv),
+ # "dqs": np.zeros(robot.model.nv),
"err_vec_ee": np.zeros((6,)),
"err_norm_ee": np.zeros((1,)),
"err_norm_base": np.zeros((1,)),
diff --git a/python/smc/multiprocessing/networking/client.py b/python/smc/multiprocessing/networking/client.py
index 1d617d261e81013308a472da618554153eb9f620..74806870ac82cc3dd3c7744a1d2126ff18f5fcc4 100644
--- a/python/smc/multiprocessing/networking/client.py
+++ b/python/smc/multiprocessing/networking/client.py
@@ -44,7 +44,14 @@ def client_receiver(args: Namespace, init_command, shm_name: str, lock):
msg_in_bytes = b""
len_size_offset = 0
while True:
- next_pos, pos = _DecodeVarint32(buffer, pos)
+ try:
+ next_pos, pos = _DecodeVarint32(buffer, pos)
+ except IndexError:
+ print(
+ "NETWORKING CLIENT_RECEIVER: got IndexError while decoding message length. \
+ this only happens if the sender died. i'll exit as well"
+ )
+ return None, -1
# TODO: either save the message chunk, or save how many initial bytes to ignore in the next message
if pos + next_pos > buffer_len:
# print("NETWORKING CLIENT: BUFFER OVERFLOW, DROPPING MSG!")
@@ -81,6 +88,9 @@ def client_receiver(args: Namespace, init_command, shm_name: str, lock):
msg_raw = s.recv(1024)
buffer += msg_raw
msg_in_bytes, pos = parse_message(buffer)
+ # -1 means an error happened
+ if pos == -1:
+ break
buffer = buffer[pos:]
dict_msg = encoder_decoder.serializedPb2MsgToDict(msg_in_bytes, msg_code)