diff --git a/examples/cart_pulling/control_sub_problems/fixed_paths/dual_arm_path_following.py b/examples/cart_pulling/control_sub_problems/fixed_paths/dual_arm_path_following.py
index 2e11a54104211e46764924edde04603c624414d5..40021e9420f356e4e10b5b85c5da0ebce7bb9361 100644
--- a/examples/cart_pulling/control_sub_problems/fixed_paths/dual_arm_path_following.py
+++ b/examples/cart_pulling/control_sub_problems/fixed_paths/dual_arm_path_following.py
@@ -38,7 +38,7 @@ if __name__ == "__main__":
x0 = np.concatenate([robot.q, robot.v])
robot._step()
- CrocoEEPathFollowingMPC(args, robot, x0, path)
+ CrocoDualEEPathFollowingMPC(args, robot, x0, path)
print("final position:", robot.T_w_e)
diff --git a/examples/cart_pulling/control_sub_problems/with_planner/base_and_dual_arm_ee_path_following_from_planner.py b/examples/cart_pulling/control_sub_problems/with_planner/base_and_dual_arm_ee_path_following_from_planner.py
new file mode 100644
index 0000000000000000000000000000000000000000..a728f36b3f9a4df81d1ab1b584895df0ca3104bb
--- /dev/null
+++ b/examples/cart_pulling/control_sub_problems/with_planner/base_and_dual_arm_ee_path_following_from_planner.py
@@ -0,0 +1,111 @@
+from smc.robots.implementations.mobile_yumi import SimulatedMobileYuMiRobotManager
+from smc import getMinimalArgParser
+from smc.path_generation.maps.premade_maps import createSampleStaticMap
+from smc.path_generation.path_math.path2d_to_6d import path2D_to_SE3
+from smc.control.optimal_control.util import get_OCP_args
+from smc.control.cartesian_space import getClikArgs
+from smc.path_generation.planner import starPlanner, getPlanningArgs
+from smc.control.optimal_control.croco_mpc_point_to_point import (
+ CrocoDualEEAndBaseP2PMPC,
+)
+from smc.control.optimal_control.croco_mpc_path_following import (
+ BaseAndDualEEPathFollowingMPC,
+)
+from smc.multiprocessing import ProcessManager
+
+import time
+import numpy as np
+from functools import partial
+import pinocchio as pin
+
+
+def get_args():
+ parser = getMinimalArgParser()
+ parser = get_OCP_args(parser)
+ parser = getClikArgs(parser) # literally just for goal error
+ parser = getPlanningArgs(parser)
+ parser.add_argument(
+ "--handlebar-height",
+ type=float,
+ default=0.5,
+ help="heigh of handlebar of the cart to be pulled",
+ )
+ parser.add_argument(
+ "--base-to-handlebar-preferred-distance",
+ type=float,
+ default=0.5,
+ help="prefered path arclength from mobile base position to handlebar",
+ )
+ args = parser.parse_args()
+ return args
+
+
+if __name__ == "__main__":
+ args = get_args()
+ assert args.robot == "myumi"
+ robot = SimulatedMobileYuMiRobotManager(args)
+ # TODO: HOW IS IT POSSIBLE THAT T_W_E IS WRONG WITHOUT STEP CALLED HERE?????????????????
+ T_absgoal_l = pin.SE3.Identity()
+ T_absgoal_l.translation[1] = 0.1
+ T_absgoal_r = pin.SE3.Identity()
+ T_absgoal_r.translation[1] = -0.1
+ robot._step()
+ robot._q[0] = 9.0
+ robot._q[1] = 4.0
+ robot._step()
+ x0 = np.concatenate([robot.q, robot.v])
+ goal = np.array([0.5, 5.5])
+
+ T_w_abs = robot.T_w_abs
+ planning_function = partial(starPlanner, goal)
+ # here we're following T_w_e reference so that's what we send
+ path_planner = ProcessManager(
+ args, planning_function, T_w_abs.translation[:2], 3, None
+ )
+ _, map_as_list = createSampleStaticMap()
+ if args.visualizer:
+ robot.sendRectangular2DMapToVisualizer(map_as_list)
+ # time.sleep(5)
+
+ T_w_abs = robot.T_w_abs
+ data = None
+ # get first path
+ ##########################################3
+ # initialize
+ ###########################################
+ while data is None:
+ path_planner.sendCommand(T_w_abs.translation[:2])
+ data = path_planner.getData()
+ # time.sleep(1 / args.ctrl_freq)
+ time.sleep(1)
+
+ _, path2D = data
+ path2D = np.array(path2D).reshape((-1, 2))
+ pathSE3 = path2D_to_SE3(path2D, args.handlebar_height)
+ if args.visualizer:
+ # TODO: document this somewhere
+ robot.visualizer_manager.sendCommand({"Mgoal": pathSE3[0]})
+ if np.linalg.norm(pin.log6(T_w_abs.actInv(pathSE3[0]))) > 1e-2:
+ print("going to initial path position")
+ p_base = pathSE3[0].translation.copy()
+ p_base[0] -= args.base_to_handlebar_preferred_distance
+ p_base[2] = 0.0
+ print(pathSE3[0].translation)
+ print(p_base)
+ CrocoDualEEAndBaseP2PMPC(
+ args, robot, pathSE3[0], T_absgoal_l, T_absgoal_r, p_base
+ )
+ print("initialized!")
+ BaseAndDualEEPathFollowingMPC(args, robot, path_planner, T_absgoal_l, T_absgoal_r)
+
+ print("final position:", robot.T_w_e)
+
+ if args.real:
+ robot.stopRobot()
+
+ if args.save_log:
+ robot._log_manager.saveLog()
+ robot._log_manager.plotAllControlLoops()
+
+ if args.visualizer:
+ robot.killManipulatorVisualizer()
diff --git a/examples/cart_pulling/disjoint_control/; b/examples/cart_pulling/disjoint_control/;
deleted file mode 100644
index b244f1397da7be08cc7d5a7ed068d1e3af2e1a95..0000000000000000000000000000000000000000
--- a/examples/cart_pulling/disjoint_control/;
+++ /dev/null
@@ -1,268 +0,0 @@
-from smc.robots.implementations import *
-
-import numpy as np
-import pinocchio as pin
-import argparse
-
-
-def getMinimalArgParser():
- """
- getDefaultEssentialArgs
- ------------------------
- returns a parser containing:
- - essential arguments (run in real or in sim)
- - parameters for (compliant)moveJ
- - parameters for (compliant)moveL
- """
- parser = argparse.ArgumentParser(
- description="Run something with \
- Simple Manipulator Control",
- formatter_class=argparse.ArgumentDefaultsHelpFormatter,
- )
- #################################################
- # general arguments: connection, plotting etc #
- #################################################
- parser.add_argument(
- "--robot",
- type=str,
- help="which robot you're running or simulating",
- default="ur5e",
- choices=["ur5e", "heron", "heronros", "gripperlessur5e", "mirros", "yumi"],
- )
- parser.add_argument(
- "--real",
- action=argparse.BooleanOptionalAction,
- help="whether you're running on the real robot or not",
- default=False,
- )
- # if this ends up working, replace --real with --mode, which can be {real, integration (simulation), debugging}
- parser.add_argument(
- "--robot-ip",
- type=str,
- help="robot's ip address (only needed if running on the real robot)",
- default="192.168.1.102",
- )
- parser.add_argument(
- "--ctrl-freq",
- type=int,
- help="frequency of the control loop. select -1 if you want to go as fast as possible (useful for running tests in sim)",
- default=500,
- )
- parser.add_argument(
- "--visualizer",
- action=argparse.BooleanOptionalAction,
- help="whether you want to visualize the manipulator and workspace with meshcat",
- default=True,
- )
- parser.add_argument(
- "--viz-update-rate",
- type=int,
- help="frequency of visual updates. visualizer and plotter update every viz-update-rate^th iteration of the control loop.",
- default=20,
- )
- parser.add_argument(
- "--plotter",
- action=argparse.BooleanOptionalAction,
- help="whether you want to have some real-time matplotlib graphs (parts of log_dict you select)",
- default=True,
- )
- parser.add_argument(
- "--gripper",
- type=str,
- help="gripper you're using (no gripper is the default)",
- default="none",
- choices=["none", "robotiq", "onrobot"],
- )
- # TODO: make controlloop manager run in a while True loop and remove this
- # ==> max-iterations actually needs to be an option. sometimes you want to simulate and exit
- # if the convergence does not happen in a reasonable amount of time,
- # ex. goal outside of workspace has been passed or something
- # =======> if it's set to 0 then the loops run infinitely long
- parser.add_argument(
- "--max-iterations",
- type=int,
- help="maximum allowable iteration number (it runs at 500Hz)",
- default=100000,
- )
- parser.add_argument(
- "--start-from-current-pose",
- action=argparse.BooleanOptionalAction,
- help="if connected to the robot, read the current pose and set it as the initial pose for the robot. \
- very useful and convenient when running simulation before running on real",
- default=False,
- )
- parser.add_argument(
- "--acceleration",
- type=float,
- help="robot's joints acceleration. scalar positive constant, max 1.7, and default 0.3. \
- BE CAREFUL WITH THIS. the urscript doc says this is 'lead axis acceleration'.\
- TODO: check what this means",
- default=0.3,
- )
- parser.add_argument(
- "--max-v-percentage",
- type=float,
- help="select the percentage of the maximum joint velocity the robot can achieve to be the control input maximum (control inputs are clipped to perc * max_v)",
- default=0.3,
- )
- parser.add_argument(
- "--debug-prints",
- action=argparse.BooleanOptionalAction,
- help="print some debug info",
- default=False,
- )
- parser.add_argument(
- "--save-log",
- action=argparse.BooleanOptionalAction,
- help="whether you want to save the log of the run. it saves \
- what you pass to ControlLoopManager. check other parameters for saving directory and log name.",
- default=False,
- )
- parser.add_argument(
- "--save-dir",
- type=str,
- help="path to where you store your logs. default is ./data, but if that directory doesn't exist, then /tmp/data is created and used.",
- default="./data",
- )
- parser.add_argument(
- "--run-name",
- type=str,
- help="name the whole run/experiment (name of log file). note that indexing of runs is automatic and under a different argument.",
- default="latest_run",
- )
- parser.add_argument(
- "--index-runs",
- action=argparse.BooleanOptionalAction,
- help="if you want more runs of the same name, this option will automatically assign an index to a new run (useful for data collection).",
- default=False,
- )
- parser.add_argument(
- "--past-window-size",
- type=int,
- help="how many timesteps of past data you want to save",
- default=5,
- )
- # maybe you want to scale the control signal (TODO: NOT HERE)
- parser.add_argument(
- "--controller-speed-scaling",
- type=float,
- default="1.0",
- help="not actually_used atm",
- )
- ########################################
- # environment interaction parameters #
- ########################################
- parser.add_argument(
- "--contact-detecting-force",
- type=float, # default=1.3, help='the force used to detect contact (collision) in the moveUntilContact function')
- default=2.8,
- help="the force used to detect contact (collision) in the moveUntilContact function",
- )
- parser.add_argument(
- "--minimum-detectable-force-norm",
- type=float,
- help="we need to disregard noise to converge despite filtering. \
- a quick fix is to zero all forces of norm below this argument threshold.",
- default=3.0,
- )
- # TODO make this work without parsing (or make it possible to parse two times)
- # if (args.gripper != "none") 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')
- parser.add_argument(
- "--visualize-collision-approximation",
- action=argparse.BooleanOptionalAction,
- help="whether you want to visualize the collision approximation used in controllers with obstacle avoidance",
- default=False,
- )
- return parser
-
-
-# TODO: make robot-independent
-def defineGoalPointCLI(robot):
- """
- defineGoalPointCLI
- ------------------
- get a nice TUI-type prompt to put in a frame goal for p2p motion.
- --> 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.
- """
- robot._step()
- q = robot.q
- # define goal
- T_w_e = robot.T_w_e
- print("You can only specify the translation right now.")
- if robot.args.real:
- print(
- "In the following, first 3 numbers are x,y,z position, and second 3 are r,p,y angles"
- )
- 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",
- *robot.data.oMi[6].translation.round(4),
- *pin.rpy.matrixToRpy(robot.data.oMi[6].rotation).round(4)
- )
- print("UR5e TCP:", *np.array(robot.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 = T_w_e.copy()
- # 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 = 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)
-
- # NOTE i'm not deepcopying this on purpose
- # but that might be the preferred thing, we'll see
- robot.Mgoal = Mgoal
- if robot.args.visualizer:
- # TODO document this somewhere
- robot.visualizer_manager.sendCommand({"Mgoal": Mgoal})
- return Mgoal
-
-
-# TODO: finish
-def getRobotFromArgs(args: argparse.Namespace) -> AbstractRobotManager:
- if args.robot == "ur5e":
- if args.real:
- return RealUR5eRobotManager(args)
- else:
- return SimulatedUR5eRobotManager(args)
- if args.robot == "heron":
- if args.real:
- pass
- # TODO: finish it
- # return RealHeronRobotManager(args)
- else:
- return SimulatedHeronRobotManager(args)
-
-
-# if args.robot == "mir":
-# return RealUR5eRobotManager(args)
-# if args.robot == "yumi":
-# return RealUR5eRobotManager(args)
diff --git a/examples/optimal_control/croco_ik_ocp.py b/examples/optimal_control/croco_ik_ocp.py
index 7e72f7488a878741e6ea53c2c3a1f7200b9fbb72..40f70f3357df988f00aacbef03a7e962552c9db7 100644
--- a/examples/optimal_control/croco_ik_ocp.py
+++ b/examples/optimal_control/croco_ik_ocp.py
@@ -39,7 +39,7 @@ if __name__ == "__main__":
# this shouldn't really depend on x0 but i can't be bothered
ocp = SingleArmIKOCP(args, robot, x0, T_w_goal)
ocp.solveInitialOCP(x0)
- reference = ocp.getSolvedReference(x0)
+ reference = ocp.getSolvedReference()
# NOTE: IF YOU PLOT SOMETHING OTHER THAN REAL-TIME PLOTTING FIRST IT BREAKS EVERYTHING
# if args.solver == "boxfddp":
diff --git a/examples/optimal_control/test_by_running.sh b/examples/optimal_control/test_by_running.sh
new file mode 100755
index 0000000000000000000000000000000000000000..dd696d3793c3fe62489313fa619e49155ccb58e9
--- /dev/null
+++ b/examples/optimal_control/test_by_running.sh
@@ -0,0 +1,66 @@
+#!/bin/bash
+# the idea here is to run all the runnable things
+# and test for syntax errors
+#
+# ################
+# single arm
+# ###############
+# ocp
+runnable="croco_ik_ocp.py --ctrl-freq=-1 --no-plotter --no-visualizer --max-iterations=2000"
+echo $runnable
+python $runnable
+
+# mpc
+runnable="croco_ik_mpc.py --max-solver-iter 10 --n-knots 30 --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+echo $runnable
+python $runnable
+
+######################
+# whole body single arm
+######################
+# ocp
+runnable="croco_ik_ocp.py --robot=heron --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+echo $runnable
+python $runnable
+# mpc
+runnable="croco_ik_mpc.py --max-solver-iter 10 --n-knots 30 --robot=heron --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+echo $runnable
+python $runnable
+
+######################
+# dual arm
+# ####################
+# ocp TODO: missing
+
+# mpc
+runnable="dual_arm_ik_mpc.py --max-solver-iter 10 --n-knots 30 --robot=yumi --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+echo $runnable
+python $runnable
+
+# ##########################
+# whole body dual arm
+# ##########################
+# ocp TODO: missing
+#
+# mpc
+runnable="dual_arm_ik_mpc.py --max-solver-iter 10 --n-knots 30 --robot=myumi --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+echo $runnable
+python $runnable
+
+##############################
+# base only
+# ##################################
+# ocp TODO: missing
+# mpc TODO: missing
+
+##############################
+# whole body single arm base + ee reference
+# ##################################
+# ocp TODO: missing
+# mpc TODO: missing
+#
+##############################
+# whole body dual arm base + ee reference
+# ##################################
+# ocp TODO: missing
+# mpc TODO: missing
diff --git a/python/smc/control/joint_space/joint_space_trajectory_following.py b/python/smc/control/joint_space/joint_space_trajectory_following.py
index dc5ee95caff0ac889d611b87bbe027f3f9eb1969..fad6b5dadc04f038a82b367395a73c79aa9f7944 100644
--- a/python/smc/control/joint_space/joint_space_trajectory_following.py
+++ b/python/smc/control/joint_space/joint_space_trajectory_following.py
@@ -1,10 +1,10 @@
-from smc.robots.robotmanager_abstract import AbstractRobotManager
+from smc.robots.abstract_robotmanager import AbstractRobotManager
from smc.control.control_loop_manager import ControlLoopManager
-from smc import getMinimalArgParser
import pinocchio as pin
import numpy as np
from functools import partial
+from collections import deque
# NOTE: it's probably a good idea to generalize this for different references:
@@ -17,8 +17,12 @@ from functools import partial
# this is because joint positions will be more accurate.
# if we integrate them with interpolated velocities.
def followKinematicJointTrajPControlLoop(
- stop_at_final: bool, robot: AbstractRobotManager, reference, i, past_data
-):
+ stop_at_final: bool,
+ robot: AbstractRobotManager,
+ reference,
+ i: int,
+ past_data: dict[str, deque[np.ndarray]],
+) -> tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]:
breakFlag = False
save_past_dict = {}
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 5e0fb60542795bc809249e236dd44f2438c33e56..cf890f7e0f8f5272209b54f67a70d39859f680f1 100644
--- a/python/smc/control/optimal_control/croco_mpc_path_following.py
+++ b/python/smc/control/optimal_control/croco_mpc_path_following.py
@@ -1,6 +1,6 @@
from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
from smc.robots.interfaces.single_arm_interface import SingleArmInterface
-from smc.robots.interfaces.whole_body_interface import SingleArmWholeBodyInterface
+from smc.robots.interfaces.whole_body_interface import SingleArmWholeBodyInterface, DualArmWholeBodyInterface
from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
from smc.control.control_loop_manager import ControlLoopManager
from smc.multiprocessing.process_manager import ProcessManager
@@ -294,24 +294,23 @@ def BaseAndEEPathFollowingFromPlannerMPCControlLoop(
# NOTE: this one is obtained as the future path from path planner
max_base_v = np.linalg.norm(robot._max_v[:2])
- path_base = path2D_to_trajectory2D(args, path2D_untimed_base, max_base_v)
- path_base = np.hstack((path_base, np.zeros((len(path_base), 1))))
-
- pathSE3_handlebar = construct_EE_path(args, p, past_data["path2D_untimed"])
+ trajectory_base = path2D_to_trajectory2D(args, path2D_untimed_base, max_base_v)
+ trajectory_base = np.hstack((trajectory_base, np.zeros((len(trajectory_base), 1))))
+ trajectorySE3_handlebar = construct_EE_path(args, p, past_data["path2D_untimed"])
if args.visualizer:
if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
- robot.visualizer_manager.sendCommand({"path": path_base})
- robot.visualizer_manager.sendCommand({"frame_path": pathSE3_handlebar})
+ robot.visualizer_manager.sendCommand({"path": trajectory_base})
+ robot.visualizer_manager.sendCommand({"frame_path": trajectorySE3_handlebar})
x0 = np.concatenate([robot.q, robot.v])
- ocp.warmstartAndReSolve(x0, data=(path_base, pathSE3_handlebar))
+ ocp.warmstartAndReSolve(x0, data=(trajectory_base, trajectorySE3_handlebar))
xs = np.array(ocp.solver.xs)
v_cmd = xs[1, robot.model.nq :]
- err_vector_ee = log6(robot.T_w_e.actInv(pathSE3_handlebar[0]))
- err_vector_base = np.linalg.norm(p - path_base[0][:2]) # z axis is irrelevant
+ err_vector_ee = log6(robot.T_w_e.actInv(trajectorySE3_handlebar[0]))
+ err_vector_base = np.linalg.norm(p - trajectory_base[0][:2]) # z axis is irrelevant
log_item = {}
log_item["err_vec_ee"] = err_vector_ee
log_item["err_norm_ee"] = np.linalg.norm(err_vector_ee).reshape((1,))
@@ -346,6 +345,7 @@ def BaseAndEEPathFollowingMPC(
are actually extracted from the state x(q,dq).
"""
+ robot._mode = SingleArmWholeBodyInterface.control_mode.whole_body
T_w_e = robot.T_w_e
x0 = np.concatenate([robot.q, robot.v])
ocp = BaseAndEEPathFollowingOCP(args, robot, x0)
@@ -393,144 +393,67 @@ def BaseAndEEPathFollowingMPC(
# TODO: the robot put in is a whole body robot,
# which you need to implement
-def BaseAndDualArmEEPathFollowingMPCControlLoop(
- args,
- robot,
+def BaseAndDualEEPathFollowingMPCControlLoop(
+ T_absgoal_l: SE3,
+ T_absgoal_r: SE3,
ocp: CrocoOCP,
- path_planner: ProcessManager,
- grasp_pose,
- goal_transform,
- iter_n,
- past_data,
-):
+ path2D_untimed_base: np.ndarray,
+ args: Namespace,
+ robot: DualArmWholeBodyInterface,
+ t: int,
+ past_data: dict[str, deque[np.ndarray]],
+) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
"""
- CrocoPathFollowingMPCControlLoop
+ BaseAndDualEEPathFollowingMPCControlLoop
-----------------------------
- end-effector(s) follow their path(s).
-
- path planner can either be a function which spits out a list of path points
- or an instance of ProcessManager which spits out path points
- by calling ProcessManager.getData()
+ cart pulling dual arm control loop
"""
- raise NotImplementedError
- breakFlag = False
- log_item = {}
- save_past_dict = {}
+ p = robot.q[:2]
- q = robot.q
- T_w_e = robot.T_w_e
- p = q[:2]
- # 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
- path_planner.sendCommand(p)
-
- ###########################
- # get path from planner #
- ###########################
- # 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.
- # but we're still fast so who cares
- 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"] = q.reshape((robot.model.nq,))
- log_item["dqs"] = robot.v.reshape((robot.model.nv,))
- return breakFlag, save_past_dict, log_item
-
- ##########################################
- # construct timed 2D path for the base #
- ##########################################
- _, path2D_untimed_base = data
- path2D_untimed_base = np.array(path2D_untimed_base).reshape((-1, 2))
- # ideally should be precomputed somewhere
+ # NOTE: this one is obtained as the future path from path planner
max_base_v = np.linalg.norm(robot._max_v[:2])
- # base just needs timing on the path
- path_base = path2D_to_trajectory2D(args, path2D_untimed_base, max_base_v)
- # and it's of height 0 (i.e. the height of base's planar joint)
- path_base = np.hstack((path_base, np.zeros((len(path_base), 1))))
-
- ###################################################
- # construct timed SE3 path for the end-effector #
- ###################################################
- # this works as follow
- # 1) find the previous path point of arclength base_to_handlebar_preferred_distance.
- # first part of the path is from there to current base position,
- # second is just the current base's plan.
- # 2) construct timing on the first part.
- # 3) join that with the already timed second part.
- # 4) turn the timed 2D path into an SE3 trajectory
-
- # 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
- path2D_handlebar_1_untimed = np.array(past_data["path2D_untimed"])
- # TODO: try this
- # path2D_handlebar_1_untimed = np.array(past_data["path2D_untimed"][handlebar_path_index:])
- # NOTE: BIG ASSUMPTION
- # let's say we're computing on time, and that's the time spacing
- # of previous path points.
- # this means you need to lower the control frequency argument
- # if you're not meeting deadlines.
- # if you really need timing information, you should actually
- # get it from ControlLoopManager instead of i,
- # but let's say this is better because you're forced to know
- # how fast you are instead of ducktaping around delays.
- # TODO: actually save timing, pass t instead of i to controlLoops
- # from controlLoopManager
- # NOTE: this might not working when rosified so check that first
- time_past = np.linspace(
- 0.0, args.past_window_size * robot.dt, args.past_window_size
- )
- s = np.linspace(0.0, args.n_knots * args.ocp_dt, args.n_knots)
- path2D_handlebar_1 = np.hstack(
- (
- np.interp(s, time_past, path2D_handlebar_1_untimed[:, 0]).reshape((-1, 1)),
- np.interp(s, time_past, path2D_handlebar_1_untimed[:, 1]).reshape((-1, 1)),
- )
- )
+ trajectory_base = path2D_to_trajectory2D(args, path2D_untimed_base, max_base_v)
+ trajectory_base = np.hstack((trajectory_base, np.zeros((len(trajectory_base), 1))))
- pathSE3_handlebar = path2D_to_SE3(path2D_handlebar_1, args.handlebar_height)
- pathSE3_handlebar_left = []
- pathSE3_handlebar_right = []
- for pathSE3 in pathSE3_handlebar:
- pathSE3_handlebar_left.append(goal_transform.act(pathSE3))
- pathSE3_handlebar_right.append(goal_transform.inverse().act(pathSE3))
+ trajectorySE3_handlebar = construct_EE_path(args, p, past_data["path2D_untimed"])
+ trajectorySE3_l = []
+ trajectorySE3_r = []
+ for traj_pose in trajectorySE3_handlebar:
+ trajectorySE3_l.append(T_absgoal_l.act(traj_pose))
+ trajectorySE3_r.append(T_absgoal_r.act(traj_pose))
if args.visualizer:
- if iter_n % 20 == 0:
- robot.visualizer_manager.sendCommand({"path": path_base})
- robot.visualizer_manager.sendCommand({"frame_path": pathSE3_handlebar})
+ if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
+ robot.visualizer_manager.sendCommand({"path": trajectory_base})
+ robot.visualizer_manager.sendCommand({"frame_path": trajectorySE3_handlebar})
x0 = np.concatenate([robot.q, robot.v])
- ocp.warmstartAndReSolve(
- x0, data=(path_base, pathSE3_handlebar_left, pathSE3_handlebar_right)
- )
+ ocp.warmstartAndReSolve(x0, data=(trajectory_base, trajectorySE3_l, trajectorySE3_r))
xs = np.array(ocp.solver.xs)
- us = np.array(ocp.solver.us)
- vel_cmds = xs[1, robot.model.nq :]
-
- robot.sendVelocityCommand(vel_cmds)
-
- log_item["qs"] = q.reshape((robot.model.nq,))
- log_item["dqs"] = robot.v.reshape((robot.model.nv,))
- return breakFlag, save_past_dict, log_item
+ v_cmd = xs[1, robot.model.nq :]
+ err_vector_ee_l = log6(robot.T_w_l.actInv(trajectorySE3_l[0]))
+ err_norm_ee_l = np.linalg.norm(err_vector_ee_l)
+ err_vector_ee_r = log6(robot.T_w_l.actInv(trajectorySE3_r[0]))
+ err_norm_ee_r = np.linalg.norm(err_vector_ee_r)
+ err_vector_base = np.linalg.norm(p - trajectory_base[0][:2]) # z axis is irrelevant
+ log_item = {}
+ log_item["err_vec_ee_l"] = err_vector_ee_l
+ log_item["err_norm_ee_l"] = err_norm_ee_l.reshape((1,))
+ log_item["err_vec_ee_r"] = err_vector_ee_r
+ log_item["err_norm_ee_r"] = err_norm_ee_r.reshape((1,))
+ log_item["err_norm_base"] = np.linalg.norm(err_vector_base).reshape((1,))
+ save_past_item = {"path2D_untimed": p}
+ return v_cmd, log_item, save_past_item
-def BaseAndDualArmEEPathFollowingMPC(args, robot, path_planner):
+def BaseAndDualEEPathFollowingMPC(
+ args: Namespace,
+ robot: DualArmWholeBodyInterface,
+ path_planner: ProcessManager | types.FunctionType,
+ T_absgoal_l: SE3,
+ T_absgoal_r: SE3,
+ run=True,
+) -> None | ControlLoopManager:
"""
CrocoEndEffectorPathFollowingMPC
-----
@@ -539,22 +462,53 @@ def BaseAndDualArmEEPathFollowingMPC(args, robot, path_planner):
a dynamics level, and velocities we command
are actually extracted from the state x(q,dq).
"""
- raise NotImplementedError("this has to be templetized like the previous cases")
+ robot._mode = DualArmWholeBodyInterface.control_mode.whole_body
+ # NOTE: i'm shoving these into the class here - bad style,
+ # but i don't
+ T_w_abs = robot.T_w_abs
x0 = np.concatenate([robot.q, robot.v])
ocp = BaseAndDualArmEEPathFollowingOCP(args, robot, x0)
ocp.solveInitialOCP(x0)
- controlLoop = partial(
- BaseAndEEPathFollowingMPCControlLoop, args, robot, ocp, path_planner
- )
- log_item = {"qs": np.zeros(robot.model.nq), "dqs": np.zeros(robot.model.nv)}
- # 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.T_w_e
- save_past_dict = {"path2D_untimed": T_w_e.translation[:2]}
+ max_base_v = np.linalg.norm(robot._max_v[:2])
+
+ path2D_handlebar = initializePastData(args, T_w_abs, robot.q[:2], float(max_base_v))
+
+ if type(path_planner) == types.FunctionType:
+ raise NotImplementedError
+ else:
+ get_position = lambda robot: robot.q[:2]
+ BaseAndDualEEPathFollowingMPCControlLoop_with_l_r = partial(BaseAndDualEEPathFollowingMPCControlLoop, T_absgoal_l, T_absgoal_r)
+ controlLoop = partial(
+ PathFollowingFromPlannerControlLoop,
+ path_planner,
+ get_position,
+ ocp,
+ BaseAndDualEEPathFollowingMPCControlLoop_with_l_r,
+ args,
+ robot,
+ )
+
+ log_item = {
+ "qs": np.zeros(robot.model.nq),
+ "dqs": np.zeros(robot.model.nv),
+ "err_vec_ee_l": np.zeros((6,)),
+ "err_norm_ee_l": np.zeros((1,)),
+ "err_vec_ee_r": np.zeros((6,)),
+ "err_norm_ee_r": np.zeros((1,)),
+ "err_norm_base": np.zeros((1,)),
+ }
+ save_past_dict = {"path2D_untimed": T_w_abs.translation[:2]}
loop_manager = ControlLoopManager(
robot, controlLoop, args, save_past_dict, log_item
)
- loop_manager.run()
+ # actually put past data into the past window
+ loop_manager.past_data["path2D_untimed"].clear()
+ loop_manager.past_data["path2D_untimed"].extend(
+ path2D_handlebar[i] for i in range(args.past_window_size)
+ )
+
+ if run:
+ loop_manager.run()
+ else:
+ return loop_manager
diff --git a/python/smc/control/optimal_control/croco_mpc_point_to_point.py b/python/smc/control/optimal_control/croco_mpc_point_to_point.py
index d79472566a2f9bdeda9e74dc0cac8c9fe218fcc1..a2cdd9f465ace9f60e30537749b6254e509342ee 100644
--- a/python/smc/control/optimal_control/croco_mpc_point_to_point.py
+++ b/python/smc/control/optimal_control/croco_mpc_point_to_point.py
@@ -220,9 +220,9 @@ def CrocoEEAndBaseP2PMPC(
def CrocoP2PDualEEAndBaseMPCControlLoop(
ocp,
- T_w_abseegoal: pin.SE3,
- T_absgoal_lgoal: pin.SE3,
- T_absgoal_rgoal: pin.SE3,
+ T_w_absgoal: pin.SE3,
+ T_absgoal_l: pin.SE3,
+ T_absgoal_r: pin.SE3,
p_basegoal: np.ndarray,
args: Namespace,
robot: DualArmWholeBodyInterface,
@@ -245,9 +245,9 @@ def CrocoP2PDualEEAndBaseMPCControlLoop(
def CrocoDualEEAndBaseP2PMPC(
args: Namespace,
robot: DualArmWholeBodyInterface,
- T_w_abseegoal: pin.SE3,
- T_absgoal_lgoal: pin.SE3,
- T_absgoal_rgoal: pin.SE3,
+ T_w_absgoal: pin.SE3,
+ T_absgoal_l: pin.SE3,
+ T_absgoal_r: pin.SE3,
p_basegoal: np.ndarray,
run=True,
) -> None | ControlLoopManager:
@@ -260,16 +260,18 @@ def CrocoDualEEAndBaseP2PMPC(
are actually extracted from the state x(q,dq)
"""
x0 = np.concatenate([robot.q, robot.v])
- goal = (T_w_abseegoal, p_basegoal)
+ T_w_lgoal = T_absgoal_l.act(T_w_absgoal)
+ T_w_rgoal = T_absgoal_r.act(T_w_absgoal)
+ goal = (T_w_lgoal, T_w_rgoal, p_basegoal)
ocp = BaseAndDualArmIKOCP(args, robot, x0, goal)
ocp.solveInitialOCP(x0)
controlLoop = partial(
DualEEAndBaseP2PCtrlLoopTemplate,
ocp,
- T_w_abseegoal,
- T_absgoal_lgoal,
- T_absgoal_rgoal,
+ T_w_absgoal,
+ T_absgoal_l,
+ T_absgoal_r,
p_basegoal,
CrocoP2PDualEEAndBaseMPCControlLoop,
args,
diff --git a/python/smc/control/optimal_control/path_following_croco_ocp.py b/python/smc/control/optimal_control/path_following_croco_ocp.py
index 698bf71ea0b5b8c25d157c6c1739199bc5bbee1f..99ed1175dfdcbc617b63166b736421d9f08f1b70 100644
--- a/python/smc/control/optimal_control/path_following_croco_ocp.py
+++ b/python/smc/control/optimal_control/path_following_croco_ocp.py
@@ -194,7 +194,7 @@ class BaseAndDualArmEEPathFollowingOCP(CrocoOCP):
super().__init__(args, robot, x0, goal)
def constructTaskObjectiveFunction(self, goal) -> None:
- T_w_e_left, T_w_e_right = self.robot.getT_w_e
+ T_w_e_left, T_w_e_right = self.robot.getLeftRightT_w_e()
path_ee_left = [T_w_e_left] * self.args.n_knots
path_ee_right = [T_w_e_right] * self.args.n_knots
path_base = [np.append(self.x0[:2], 0.0)] * self.args.n_knots
@@ -202,7 +202,7 @@ class BaseAndDualArmEEPathFollowingOCP(CrocoOCP):
for i in range(self.args.n_knots):
l_eePoseResidual = crocoddyl.ResidualModelFramePlacement(
self.state,
- self.robot.model.l_ee_frame_id,
+ self.robot.l_ee_frame_id,
path_ee_left[i],
self.state.nv,
)
@@ -212,7 +212,7 @@ class BaseAndDualArmEEPathFollowingOCP(CrocoOCP):
)
r_eePoseResidual = crocoddyl.ResidualModelFramePlacement(
self.state,
- self.robot.model.r_ee_frame_id,
+ self.robot.r_ee_frame_id,
path_ee_right[i],
self.state.nv,
)
@@ -221,7 +221,7 @@ class BaseAndDualArmEEPathFollowingOCP(CrocoOCP):
"r_ee_pose" + str(i), r_eeTrackingCost, self.args.ee_pose_cost
)
baseTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
- self.state, self.robot.model.base_frame_id, path_base[i], self.state.nv
+ self.state, self.robot.base_frame_id, path_base[i], self.state.nv
)
baseTrackingCost = crocoddyl.CostModelResidual(
self.state, baseTranslationResidual
diff --git a/python/smc/control/optimal_control/point_to_point_croco_ocp.py b/python/smc/control/optimal_control/point_to_point_croco_ocp.py
index 1b9625dc447bb6e835c8919bf14357e8e0fb0216..519458f91f113b5a3ca333abfdb42752716f83c5 100644
--- a/python/smc/control/optimal_control/point_to_point_croco_ocp.py
+++ b/python/smc/control/optimal_control/point_to_point_croco_ocp.py
@@ -214,8 +214,6 @@ class BaseAndDualArmIKOCP(DualArmIKOCP):
robot: DualArmWholeBodyInterface,
x0: np.ndarray,
goal,
- # T_w_eegoal: pin.SE3,
- # p_basegoal: np.ndarray,
):
# T_w_eegoal, p_basegoal = goal
super().__init__(args, robot, x0, goal)