diff --git a/examples/cart_pulling/path_following_from_planner.py b/examples/cart_pulling/path_following_from_planner.py
index 54581b9e40eaf4a228611c772d1bac20a3b1506e..8470014f5bbff713a4a54552c6eed5b33ef36788 100644
--- a/examples/cart_pulling/path_following_from_planner.py
+++ b/examples/cart_pulling/path_following_from_planner.py
@@ -32,8 +32,8 @@ def get_args():
if __name__ == "__main__":
args = get_args()
robot = getRobotFromArgs(args)
- robot._q[0] = 9.0
- robot._q[1] = 4.0
+ # 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])
diff --git a/examples/log_analysis/log_analysis_example.py b/examples/log_analysis/log_analysis_example.py
index 4d92c970efc1da3b9dc3fdb05654d9cb957c813b..ea531f16c09e8f21aaabd02bae94ca18669bded9 100644
--- a/examples/log_analysis/log_analysis_example.py
+++ b/examples/log_analysis/log_analysis_example.py
@@ -1,16 +1,15 @@
import pinocchio as pin
import numpy as np
import argparse
-from smc.util.logging_utils import LogManager
-from smc.visualize.manipulator_comparison_visualizer import (
+from smc.logging.logger import Logger
+from smc.visualization.manipulator_comparison_visualizer import (
getLogComparisonArgs,
- ManipulatorComparisonManager,
)
if __name__ == "__main__":
args = getLogComparisonArgs()
- log_manager = LogManager(None)
+ log_manager = Logger(None)
log_manager.loadLog(args.log_file1)
# mcm = ManipulatorComparisonManager(args)
# mcm.visualizeWholeRuns()
diff --git a/python/smc/control/optimal_control/abstract_croco_ocp.py b/python/smc/control/optimal_control/abstract_croco_ocp.py
index fd68018311921e0e560de9a3d89a49e111a969a6..59a267fdc2839ca82ea83931f747ddfb9e0db901 100644
--- a/python/smc/control/optimal_control/abstract_croco_ocp.py
+++ b/python/smc/control/optimal_control/abstract_croco_ocp.py
@@ -105,10 +105,10 @@ class CrocoOCP(abc.ABC):
# the first state is unlimited there idk what that means really,
# but the arm's base isn't doing a full rotation anyway, let alone 2 or more
self.xlb = np.concatenate(
- [self.robot.model.lowerPositionLimit, -1 * self.robot.model.velocityLimit]
+ [self.robot.model.lowerPositionLimit, -1 * self.robot._max_v]
)
self.xub = np.concatenate(
- [self.robot.model.upperPositionLimit, self.robot.model.velocityLimit]
+ [self.robot.model.upperPositionLimit, self.robot._max_v]
)
# NOTE: in an ideal universe this is handled elsewhere
@@ -135,6 +135,8 @@ class CrocoOCP(abc.ABC):
):
self.xlb = self.xlb[1:]
self.xub = self.xub[1:]
+ print(self.xlb)
+ print(self.xub)
def constructConstraints(self) -> None:
if self.solver_name == "boxfddp":
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 2ce182ff2d0830430f0514728d23e93c2d2b336e..1f1338c20ee3cfece648c6e9dcba1b91cfe8c273 100644
--- a/python/smc/control/optimal_control/croco_mpc_path_following.py
+++ b/python/smc/control/optimal_control/croco_mpc_path_following.py
@@ -1,4 +1,5 @@
from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+from smc.control.optimal_control.croco_mpc_point_to_point import CrocoIKMPC
from smc.control.control_loop_manager import ControlLoopManager
from smc.multiprocessing.process_manager import ProcessManager
from smc.control.optimal_control.path_following_croco_ocp import (
@@ -16,6 +17,9 @@ import numpy as np
from functools import partial
import types
import importlib.util
+from argparse import Namespace
+import pinocchio as pin
+import time
if importlib.util.find_spec("mim_solvers"):
try:
@@ -45,7 +49,7 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(
"""
breakFlag = False
log_item = {}
- save_past_dict = {}
+ save_past_item = {}
q = robot.q
T_w_e = robot.T_w_e
@@ -65,7 +69,7 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(
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
+ return breakFlag, save_past_item, log_item
if data == "done":
breakFlag = True
@@ -80,10 +84,11 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(
# create a 3D reference out of the path
pathSE3 = path2D_to_SE3_fixed(path2D, args.handlebar_height)
+ print("following from", pathSE3[0])
+ exit()
# TODO: EVIL AND HAS TO BE REMOVED FROM HERE
if args.visualizer:
- # if t % 20 == 0:
if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
robot.visualizer_manager.sendCommand({"frame_path": pathSE3})
@@ -115,19 +120,45 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(
log_item["qs"] = q.reshape((robot.model.nq,))
log_item["dqs"] = robot.v.reshape((robot.model.nv,))
- return breakFlag, save_past_dict, log_item
+ return breakFlag, save_past_item, log_item
-def CrocoEndEffectorPathFollowingMPC(args, robot, x0, path_planner):
+def CrocoEndEffectorPathFollowingMPC(
+ args: Namespace,
+ robot: SingleArmInterface,
+ x0: np.ndarray,
+ path_planner: ProcessManager,
+):
"""
CrocoEndEffectorPathFollowingMPC
-----
- run mpc for a point-to-point inverse kinematics.
- note that the actual problem is solved on
- a dynamics level, and velocities we command
- are actually extracted from the state x(q,dq).
+ follow a fixed pre-determined path, or a path received from a planner.
+ the path does NOT need to start from your current pose - you need to get to it yourself.
"""
+ T_w_e = robot.T_w_e
+ data = None
+ # get first path
+ while data is None:
+ path_planner.sendCommand(T_w_e.translation[:2])
+ data = path_planner.getData()
+ # time.sleep(1 / args.ctrl_freq)
+ time.sleep(1)
+
+ _, path2D = data
+ print(path2D[0])
+ path2D = np.array(path2D).reshape((-1, 2))
+ pathSE3 = path2D_to_SE3_fixed(path2D, args.handlebar_height)
+ if args.visualizer:
+ # TODO: document this somewhere
+ robot.visualizer_manager.sendCommand({"Mgoal": pathSE3[0]})
+ print("i'm at", T_w_e)
+ print("going to", pathSE3[0])
+ if np.linalg.norm(pin.log6(T_w_e.actInv(pathSE3[0]))) > 1e-2:
+ print("running")
+ print("error", np.linalg.norm(pin.log6(T_w_e.actInv(pathSE3[0]))))
+ CrocoIKMPC(args, robot, pathSE3[0])
+ print("initialized!")
ocp = CrocoEEPathFollowingOCP(args, robot, x0)
solver = ocp.getSolver()
@@ -143,9 +174,9 @@ def CrocoEndEffectorPathFollowingMPC(args, robot, x0, path_planner):
CrocoEndEffectorPathFollowingMPCControlLoop, args, robot, solver, path_planner
)
log_item = {"qs": np.zeros(robot.model.nq), "dqs": np.zeros(robot.model.nv)}
- save_past_dict = {}
+ save_past_item = {"T_w_e": robot.T_w_e}
loop_manager = ControlLoopManager(
- robot, controlLoop, args, save_past_dict, log_item
+ robot, controlLoop, args, save_past_item, log_item
)
loop_manager.run()
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 1f643a5b38837c0daf12a9d3c8914fd1365e9af2..30882035a5c396066e0825852da8d26be3522354 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
@@ -44,11 +44,12 @@ def CrocoIKMPCControlLoop(args, robot: SingleArmInterface, solver, T_w_goal, _,
solver.solve(xs_init, us_init, args.max_solver_iter)
xs = np.array(solver.xs)
us = np.array(solver.us)
+ # NOTE: for some reason the first value is always some wild bs
vel_cmds = xs[1, robot.model.nq :]
robot.sendVelocityCommand(vel_cmds)
log_item["qs"] = x0[: robot.model.nq]
- log_item["dqs"] = x0[robot.model.nv :]
+ log_item["dqs"] = x0[robot.model.nq :]
log_item["dqs_cmd"] = vel_cmds
log_item["u_tau"] = us[0]
@@ -78,7 +79,7 @@ def CrocoIKMPC(args, robot, T_w_goal, run=True):
controlLoop = partial(CrocoIKMPCControlLoop, args, robot, solver, T_w_goal)
log_item = {
"qs": np.zeros(robot.model.nq),
- "dqs": np.zeros(robot.model.nq),
+ "dqs": np.zeros(robot.model.nv),
"dqs_cmd": np.zeros(robot.model.nv), # we're assuming full actuation here
"u_tau": np.zeros(robot.model.nv),
}
diff --git a/python/smc/robots/implementations/ur5e.py b/python/smc/robots/implementations/ur5e.py
index ba9768a545ac675a39896b902be43a411e64203c..dbbcb888cab1674ca19dee53148780db640752e2 100644
--- a/python/smc/robots/implementations/ur5e.py
+++ b/python/smc/robots/implementations/ur5e.py
@@ -40,6 +40,7 @@ class RobotManagerUR5e(ForceTorqueOnSingleArmWrist):
if args.debug_prints:
print("RobotManagerUR5e init")
self._model, self._collision_model, self._visual_model, self._data = get_model()
+ self._ee_frame_id = self.model.getFrameId("tool0")
self._MAX_ACCELERATION = 1.7 # const
self._MAX_QD = 3.14 # const
super().__init__(args)
diff --git a/python/smc/robots/interfaces/single_arm_interface.py b/python/smc/robots/interfaces/single_arm_interface.py
index c486f974f34f286ddfb5ffbcc5522a16bb268351..b6f6e46f15f8a23eca52b0a75b836b2ef4e55b0d 100644
--- a/python/smc/robots/interfaces/single_arm_interface.py
+++ b/python/smc/robots/interfaces/single_arm_interface.py
@@ -7,7 +7,7 @@ class SingleArmInterface(AbstractRobotManager):
def __init__(self, args):
if args.debug_prints:
print("SingleArmInterface init")
- self._ee_frame_id: int = self.model.getFrameId("tool0")
+ self._ee_frame_id: int
self._T_w_e: pin.SE3
super().__init__(args)
diff --git a/python/smc/robots/robotmanager_abstract.py b/python/smc/robots/robotmanager_abstract.py
index 0e6cee64fd793b776d2d509c94c7b95a7616c856..2b94efd52e249f42b0b7c02c6a673fb423f01c26 100644
--- a/python/smc/robots/robotmanager_abstract.py
+++ b/python/smc/robots/robotmanager_abstract.py
@@ -86,6 +86,8 @@ class AbstractRobotManager(abc.ABC):
# TODO: each robot should know which grippers are available
# and set this there.
self.gripper = None
+ # initialize things that depend on q here
+ self._step()
####################################################################
# processes and utilities robotmanager owns #
diff --git a/python/smc/visualization/manipulator_comparison_visualizer.py b/python/smc/visualization/manipulator_comparison_visualizer.py
index 9c1785ca85807110a8e48f41de63e33c349a93d8..5c07d23e6d6b17897583c031948ad2b8a8ceb8e9 100644
--- a/python/smc/visualization/manipulator_comparison_visualizer.py
+++ b/python/smc/visualization/manipulator_comparison_visualizer.py
@@ -1,4 +1,4 @@
-from smc.robots.implementations.simulated_robot import SimulatedRobotManager
+from smc import getRobotFromArgs
from smc.robots.utils import getMinimalArgParser
from smc.multiprocessing.process_manager import ProcessManager
from smc.logging.logger import Logger
@@ -40,8 +40,8 @@ class ManipulatorComparisonManager:
args.pinocchio_only = True
args.simulation = False
- self.robot1 = SimulatedRobotManager(args)
- self.robot2 = SimulatedRobotManager(args)
+ self.robot1 = getRobotFromArgs(args)
+ self.robot2 = getRobotFromArgs(args)
# no two loops will have the same amount of timesteps.
# we need to store the last available step for both robots.
@@ -49,13 +49,13 @@ class ManipulatorComparisonManager:
self.lastq2 = np.zeros(self.robot2.model.nq)
if os.path.exists(args.log_file1):
- self.logm1 = LogManager(None)
+ self.logm1 = Logger(None)
self.logm1.loadLog(args.log_file1)
else:
print("you did not give me a valid path for log1, exiting")
exit()
if os.path.exists(args.log_file2):
- self.logm2 = LogManager(None)
+ self.logm2 = Logger(None)
self.logm2.loadLog(args.log_file2)
else:
print("you did not give me a valid path for log2, exiting")