From 3b2dd24ae469d0888bd309c10dfbadc06d4e2ebc Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Thu, 6 Mar 2025 01:26:05 +0100
Subject: [PATCH] still debugging why navigation mpc stops for no reason.
almost done with cartesian path following - with correctly formulated QP it
solves underactuation. did some nice refactoring of path-following mpcs
---
docker/Dockerfile | 45 ++---
docker/dot_files_for_docker/.zshrc | 7 +
examples/navigation/mobile_base_navigation.py | 5 +-
...co_ik_mpc.py => croco_single_ee_ik_mpc.py} | 3 -
...co_ik_ocp.py => croco_single_ee_ik_ocp.py} | 4 +-
.../optimal_control/path_following_mpc.py | 2 -
examples/optimal_control/test_by_running.sh | 14 +-
python/setup.py | 54 ++---
.../cartesian_space_trajectory_following.py | 157 +++++++-------
.../path_following_template.py | 7 +-
.../optimal_control/abstract_croco_ocp.py | 4 +
.../croco_mpc_path_following.py | 27 ++-
.../path_following_croco_ocp.py | 191 ++++--------------
.../point_to_point_croco_ocp.py | 81 +++++++-
python/smc/control/optimal_control/util.py | 12 +-
.../interfaces/mobile_base_interface.py | 11 +
16 files changed, 304 insertions(+), 320 deletions(-)
rename examples/optimal_control/{croco_ik_mpc.py => croco_single_ee_ik_mpc.py} (95%)
rename examples/optimal_control/{croco_ik_ocp.py => croco_single_ee_ik_ocp.py} (97%)
diff --git a/docker/Dockerfile b/docker/Dockerfile
index c78ced7..5e76c53 100644
--- a/docker/Dockerfile
+++ b/docker/Dockerfile
@@ -1,11 +1,17 @@
-FROM ubuntu:jammy
+#FROM ubuntu:jammy # required for ros
+FROM ubuntu:noble
LABEL org.opencontainers.image.authors="marko.guberina@control.lth.se"
-# install python3-tk without questions
+# NOTE: this is here just to install python3-tk without questions
ARG DEBIAN_FRONTEND=noninteractive
ENV TZ=Europe/Stockholm
+# pinocchio install
+RUN apt install -qqy lsb-release curl
+RUN mkdir -p /etc/apt/keyrings curl http://robotpkg.openrobots.org/packages/debian/robotpkg.asc | tee /etc/apt/keyrings/robotpkg.asc
+RUN echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -cs) robotpkg" | sudo tee /etc/apt/sources.list.d/robotpkg.list
+
RUN apt-get update && apt-get install -y --no-install-recommends \
python3 \
python3-pip \
@@ -27,14 +33,17 @@ RUN apt-get update && apt-get install -y --no-install-recommends \
zsh-syntax-highlighting \
libarchive-tools \
libxcb-cursor0\
- python3-python-qt-binding
-
+ python3-python-qt-binding \
+ robotpkg-py3*-pinocchio
+
+
+
# qt-binding is a really unnecessary 300MB, but i don't want
# to do more matplotlib hacks
# if you want manuals:
#RUN sed -i 's:^path-exclude=/usr/share/man:#path-exclude=/usr/share/man:' \
# /etc/dpkg/dpkg.cfg.d/excludes
-# we're running out of space on the docker.control
+# --> we're running out of space on the docker.control
# and noone is reading manuals anyway
#RUN yes | unminimize
@@ -54,33 +63,13 @@ COPY --chown=student /dot_files_for_docker/.zshrc /home/student/
COPY --chown=student /dot_files_for_docker/global_extra_conf.py /home/student/
-# sh does not have sourcing
-# and some packages (conda) want shell environment variables
-# (which i can say a lot about, but can't do anything about)
-# ((the only reason to even use conda is to not have to compile pinocchio))
-SHELL ["/bin/bash", "--login", "-c"]
-#SHELL ["/bin/bash"]
-
# this is enough to run clik
WORKDIR /home/student/
USER student
-# TODO: install casadi and pinochio 3.0+
-# TODO: verify this stuff below works
-# --> this can be done with conda
-RUN mkdir -p ~/miniconda3
-RUN wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh -O /home/student/miniconda3/miniconda.sh
-RUN bash /home/student/miniconda3/miniconda.sh -b -u -p ~/miniconda3
-RUN rm /home/student/miniconda3/miniconda.sh
-ENV PATH=/home/student/miniconda3/bin:$PATH
-RUN source /home/student/miniconda3/bin/activate
RUN pip install -e ./SimpleManipulatorControl/python/
-RUN conda config --add channels conda-forge
-#RUN conda install --solver=classic conda-forge::conda-libmamba-solver conda-forge::libmamba conda-forge::libmambapy conda-forge::libarchive
-#RUN conda install --solver=classic -y pinocchio crocoddyl -c conda-forge
-RUN conda install -y pinocchio crocoddyl -c conda-forge
-#RUN conda install -y opencv
-RUN pip install matplotlib meshcat ur_rtde argcomplete \
+RUN pip install crocoddyl matplotlib meshcat ur_rtde \
qpsolvers ecos example_robot_data meshcat_shapes \
- pyqt6 opencv-python
+ pyqt6 opencv-python qpsolvers quadprog \
+ proxsuite casadi pin-pink
RUN vam install python-jedi && vam install youcompleteme
diff --git a/docker/dot_files_for_docker/.zshrc b/docker/dot_files_for_docker/.zshrc
index 6ba1e66..b75ceee 100644
--- a/docker/dot_files_for_docker/.zshrc
+++ b/docker/dot_files_for_docker/.zshrc
@@ -109,6 +109,13 @@ export SUDO_EDITOR="$VISUAL"
#export CC=/usr/bin/clang
#export CXX=/usr/bin/clang++
+# pinocchio installed via apt has these locations
+export PATH=/opt/openrobots/bin:$PATH
+export PKG_CONFIG_PATH=/opt/openrobots/lib/pkgconfig:$PKG_CONFIG_PATH
+export LD_LIBRARY_PATH=/opt/openrobots/lib:$LD_LIBRARY_PATH
+export PYTHONPATH=/opt/openrobots/lib/python3.10/site-packages:$PYTHONPATH # Adapt your desired python version here
+export CMAKE_PREFIX_PATH=/opt/openrobots:$CMAKE_PREFIX_PATH
+
# Load aliases and shortcuts if existent.
[ -f "$HOME/.config/shortcutrc" ] && source "$HOME/.config/shortcutrc"
diff --git a/examples/navigation/mobile_base_navigation.py b/examples/navigation/mobile_base_navigation.py
index d218389..685637d 100644
--- a/examples/navigation/mobile_base_navigation.py
+++ b/examples/navigation/mobile_base_navigation.py
@@ -5,6 +5,9 @@ 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.control.cartesian_space.cartesian_space_trajectory_following import (
+ cartesianPathFollowingWithPlanner,
+)
from smc.path_generation.planner import starPlanner, getPlanningArgs
from smc.control.optimal_control.croco_mpc_point_to_point import CrocoBaseP2PMPC
from smc.control.optimal_control.croco_mpc_path_following import (
@@ -15,7 +18,6 @@ from smc.util.draw_path import drawPath
import numpy as np
from functools import partial
-import pinocchio as pin
import argparse
import matplotlib
@@ -117,6 +119,7 @@ if __name__ == "__main__":
path_planner = partial(fixedPathPlanner, path2D)
CrocoBasePathFollowingMPC(args, robot, x0, path_planner)
+ # cartesianPathFollowingWithPlanner(args, robot, path_planner)
if args.real:
robot.stopRobot()
diff --git a/examples/optimal_control/croco_ik_mpc.py b/examples/optimal_control/croco_single_ee_ik_mpc.py
similarity index 95%
rename from examples/optimal_control/croco_ik_mpc.py
rename to examples/optimal_control/croco_single_ee_ik_mpc.py
index 71f1c43..bd25d55 100644
--- a/examples/optimal_control/croco_ik_mpc.py
+++ b/examples/optimal_control/croco_single_ee_ik_mpc.py
@@ -30,9 +30,6 @@ if __name__ == "__main__":
CrocoEEP2PMPC(args, robot, T_w_goal)
- print("final position:")
- print(robot.T_w_e)
-
if args.real:
robot.stopRobot()
diff --git a/examples/optimal_control/croco_ik_ocp.py b/examples/optimal_control/croco_single_ee_ik_ocp.py
similarity index 97%
rename from examples/optimal_control/croco_ik_ocp.py
rename to examples/optimal_control/croco_single_ee_ik_ocp.py
index 40f70f3..0b85c48 100644
--- a/examples/optimal_control/croco_ik_ocp.py
+++ b/examples/optimal_control/croco_single_ee_ik_ocp.py
@@ -51,8 +51,8 @@ if __name__ == "__main__":
# and because it is sampled at a much lower frequency
followKinematicJointTrajP(args, robot, reference)
- print("final position:")
- print(robot.T_w_e)
+ # print("final position:")
+ # print(robot.T_w_e)
if args.real:
robot.stopRobot()
diff --git a/examples/optimal_control/path_following_mpc.py b/examples/optimal_control/path_following_mpc.py
index 2e11a54..5ec64e7 100644
--- a/examples/optimal_control/path_following_mpc.py
+++ b/examples/optimal_control/path_following_mpc.py
@@ -40,8 +40,6 @@ if __name__ == "__main__":
CrocoEEPathFollowingMPC(args, robot, x0, path)
- print("final position:", robot.T_w_e)
-
if args.real:
robot.stopRobot()
diff --git a/examples/optimal_control/test_by_running.sh b/examples/optimal_control/test_by_running.sh
index dd696d3..dc04fc8 100755
--- a/examples/optimal_control/test_by_running.sh
+++ b/examples/optimal_control/test_by_running.sh
@@ -1,17 +1,17 @@
#!/bin/bash
# the idea here is to run all the runnable things
# and test for syntax errors
-#
+# TODO: make these work with different modes by making robot.model a property which outputs truncated models
# ################
# single arm
# ###############
# ocp
-runnable="croco_ik_ocp.py --ctrl-freq=-1 --no-plotter --no-visualizer --max-iterations=2000"
+runnable="croco_single_ee_ik_ocp.py --ctrl-freq=-1 --no-plotter --no-visualizer --max-iterations=2"
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"
+runnable="croco_single_ee_ik_mpc.py --max-solver-iter 10 --n-knots 30 --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2"
echo $runnable
python $runnable
@@ -19,11 +19,11 @@ python $runnable
# whole body single arm
######################
# ocp
-runnable="croco_ik_ocp.py --robot=heron --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+runnable="croco_single_ee_ik_ocp.py --robot=heron --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2"
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"
+runnable="croco_single_ee_ik_mpc.py --max-solver-iter 10 --n-knots 30 --robot=heron --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2"
echo $runnable
python $runnable
@@ -33,7 +33,7 @@ python $runnable
# 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"
+runnable="dual_arm_ik_mpc.py --max-solver-iter 10 --n-knots 30 --robot=yumi --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2"
echo $runnable
python $runnable
@@ -43,7 +43,7 @@ python $runnable
# 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"
+runnable="dual_arm_ik_mpc.py --max-solver-iter 10 --n-knots 30 --robot=myumi --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2"
echo $runnable
python $runnable
diff --git a/python/setup.py b/python/setup.py
index ac19f1e..e6568b2 100644
--- a/python/setup.py
+++ b/python/setup.py
@@ -1,35 +1,39 @@
from setuptools import setup, find_packages
-setup(name='smc',
- version='0.3',
- description='Simple Manipulator Control (SMC) - simplest possible framework for robot control.',
- author='Marko Guberina',
- url='https://gitlab.control.lth.se/marko-g/ur_simple_control',
- packages=['smc'],
- # package_dir={"": "ur_simple_control"},
- package_data={
- 'smc.robots.robot_descriptions': ['*'],
- },
- zip_safe=False)
+
+setup(
+ name="smc",
+ version="0.3",
+ description="Simple Manipulator Control (SMC) - simplest possible framework for robot control.",
+ author="Marko Guberina",
+ url="https://gitlab.control.lth.se/marko-g/ur_simple_control",
+ packages=["smc"],
+ # package_dir={"": "ur_simple_control"},
+ package_data={
+ "smc.robots.robot_descriptions": ["*"],
+ },
+ zip_safe=False,
+)
# NOTE: if you want to switch to the toml thing,
# here you go, knock yourself out, didn't really work for me,
# and i don't care at this stage
- # add other ones and test, check .toml file for more
- # dependencies need to be taken care of separately
- # because you don't want to install pinocchio via pip
- #install_requires=['numpy'],
- # broken, but if you'll want to switch later here you go
- #packages=find_packages(where='src'),
- #package_data={'ur_simple_control': ['clik/*', 'dmp/*', 'util/*']}
- #)
+# add other ones and test, check .toml file for more
+# dependencies need to be taken care of separately
+# because you don't want to install pinocchio via pip
+# install_requires=['numpy'],
+# broken, but if you'll want to switch later here you go
+# packages=find_packages(where='src'),
+# package_data={'ur_simple_control': ['clik/*', 'dmp/*', 'util/*']}
+# )
# dependencies:
# pinocchio - robot math
# numpy - math
# matplotlib - plotter
# meshcat, meshcat_shapes - visualizer
-# optional ik solver: qpsolvers, quadprog, proxsuite
-# optional for opc/mpc: crocoddyl
-# optional for path planning: albin's path planning repos: starworlds, star_navigation, tunnel_mpc
-# optional for visual servoing: cv2
-# optional for different ocp: casadi AND pinocchio > 3.0
-# optional for yumi, mobile yumi, and heron: ros2
+# [optional] UR robots control: ur_rtde
+# [optional] ik solvers: qpsolvers, quadprog, proxsuite, ecos, pink
+# [optional] for opc/mpc: crocoddyl
+# [optional] for path planning: albin's path planning repos: starworlds, star_navigation, tunnel_mpc (NOTE all these repos are copy-pasted to path_generation atm)
+# [optional] for visual servoing: opencv-python
+# [optional] for different ocp: casadi (requires pinocchio > 3.0)
+# [optional] for yumi, mobile yumi, and heron: ros2
diff --git a/python/smc/control/cartesian_space/cartesian_space_trajectory_following.py b/python/smc/control/cartesian_space/cartesian_space_trajectory_following.py
index cefb4bb..b278695 100644
--- a/python/smc/control/cartesian_space/cartesian_space_trajectory_following.py
+++ b/python/smc/control/cartesian_space/cartesian_space_trajectory_following.py
@@ -1,86 +1,93 @@
from smc.control.control_loop_manager import ControlLoopManager
from smc.robots.abstract_robotmanager import AbstractRobotManager
from smc.multiprocessing.process_manager import ProcessManager
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+from smc.control.controller_templates.path_following_template import (
+ PathFollowingFromPlannerCtrllLoopTemplate,
+)
+from smc.control.cartesian_space.ik_solvers import getIKSolver, dampedPseudoinverse
+
from functools import partial
import pinocchio as pin
import numpy as np
+from argparse import Namespace
+from collections import deque
+from typing import Callable
+import types
+
# TODO: update this with all the changes
# from smc.path_generation.path_math.path_to_trajectory import path2D_to_timed_SE3
#
#
-# def cartesianPathFollowingWithPlannerControlLoop(
-# args, robot: AbstractRobotManager, path_planner: ProcessManager, i, past_data
-# ):
-# """
-# cartesianPathFollowingWithPlanner
-# -----------------------------
-# end-effector(s) follow their path(s) according to what a 2D path-planner spits out
-# """
-# breakFlag = False
-# log_item = {}
-# save_past_dict = {}
-#
-# q = robot.q
-# T_w_e = robot.T_w_e
-# p = T_w_e.translation[:2]
-# path_planner.sendFreshestCommand({"p": p})
-#
-# # NOTE: it's pointless to recalculate the path every time
-# # if it's the same 2D path
-# data = path_planner.getData()
-# if data == None:
-# if args.debug_prints:
-# print("got no path so no doing anything")
-# robot.sendVelocityCommand(np.zeros(robot.model.nv))
-# log_item["qs"] = q.reshape((robot.model.nq,))
-# log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
-# return breakFlag, save_past_dict, log_item
-# if data == "done":
-# breakFlag = True
-#
-# path_pol, path2D_untimed = data
-# path2D_untimed = np.array(path2D_untimed).reshape((-1, 2))
-# # should be precomputed somewhere but this is nowhere near the biggest problem
-# max_base_v = np.linalg.norm(robot.model.robot.model.velocityLimit[:2])
-#
-# # 1) make 6D path out of [[x0,y0],...]
-# # that represents the center of the cart
-# pathSE3 = path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v)
-# # print(path2D_untimed)
-# # for pp in pathSE3:
-# # print(pp.translation)
-# # TODO: EVIL AND HAS TO BE REMOVED FROM HERE
-# if args.visualizer:
-# robot.visualizer_manager.sendCommand({"path": pathSE3})
-#
-# J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
-# # NOTE: obviously not path following but i want to see things working here
-# SEerror = T_w_e.actInv(pathSE3[-1])
-# err_vector = pin.log6(SEerror).vector
-# lb = -1 * robot.model.robot.model.velocityLimit
-# lb[1] = -0.001
-# ub = robot.model.robot.model.velocityLimit
-# ub[1] = 0.001
-# # vel_cmd = invKinmQP(J, err_vector, lb=lb, ub=ub)
-# vel_cmd = dampedPseudoinverse(0.002, J, err_vector)
-# robot.sendVelocityCommand(vel_cmd)
-#
-# log_item["qs"] = q.reshape((robot.model.nq,))
-# log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
-# # time.sleep(0.01)
-# return breakFlag, save_past_dict, log_item
-#
-#
-# def cartesianPathFollowingWithPlanner(
-# args, robot: AbstractRobotManager, path_planner: ProcessManager
-# ):
-# controlLoop = partial(
-# cartesianPathFollowingWithPlannerControlLoop, args, robot, path_planner
-# )
-# log_item = {"qs": np.zeros(robot.model.nq), "dqs": np.zeros(robot.model.nv)}
-# save_past_dict = {}
-# loop_manager = ControlLoopManager(
-# robot, controlLoop, args, save_past_dict, log_item
-# )
-# loop_manager.run()
+def cartesianPathFollowingControlLoop(
+ ik_solver: Callable[[np.ndarray, np.ndarray], np.ndarray],
+ path: list[pin.SE3],
+ args: Namespace,
+ robot: SingleArmInterface,
+ t: int,
+ _: dict[str, deque[np.ndarray]],
+) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
+ """
+ cartesianPathFollowingControlLoop
+ -----------------------------
+ end-effector(s) follow their path(s) according to what a 2D path-planner spits out
+ """
+
+ # TODO: arbitrary bs, read a book and redo this
+ # NOTE: assuming the first path point coincides with current pose
+ SEerror = robot.T_w_e.actInv(path[1])
+ V_path = pin.log6(path[0].actInv(path[1])).vector
+ err_vector = pin.log6(SEerror).vector + V_path
+ J = robot.getJacobian()
+ v_cmd = ik_solver(J, err_vector)
+
+ if v_cmd is None:
+ print(
+ t,
+ "the controller you chose produced None as output, using dampedPseudoinverse instead",
+ )
+ v_cmd = dampedPseudoinverse(1e-2, J, err_vector)
+ else:
+ if args.debug_prints:
+ print(t, "ik solver success")
+
+ # maybe visualize the closest path point instead? the path should be handled
+ # by the path planner
+ # if args.visualizer:
+ # if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
+ # robot.visualizer_manager.sendCommand({"frame_path": path_EE_SE3})
+
+ return v_cmd, {}, {}
+
+
+def cartesianPathFollowingWithPlanner(
+ args: Namespace,
+ robot: SingleArmInterface,
+ path_planner: ProcessManager | types.FunctionType,
+ run=True,
+) -> None | ControlLoopManager:
+ ik_solver = getIKSolver(args, robot)
+ get_position = lambda robot: robot.T_w_e.translation[:2]
+ controlLoop = partial(
+ PathFollowingFromPlannerCtrllLoopTemplate,
+ path_planner,
+ get_position,
+ ik_solver,
+ cartesianPathFollowingControlLoop,
+ args,
+ robot,
+ )
+ log_item = {
+ "qs": np.zeros(robot.model.nq),
+ "dqs": np.zeros(robot.model.nv),
+ "err_vec_ee": np.zeros(6),
+ }
+ save_past_item = {}
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_item, log_item
+ )
+ if run:
+ loop_manager.run()
+ else:
+ return loop_manager
diff --git a/python/smc/control/controller_templates/path_following_template.py b/python/smc/control/controller_templates/path_following_template.py
index 1ff64ef..13915f6 100644
--- a/python/smc/control/controller_templates/path_following_template.py
+++ b/python/smc/control/controller_templates/path_following_template.py
@@ -6,19 +6,20 @@ from argparse import Namespace
from typing import Any, Callable
import numpy as np
from collections import deque
+from pinocchio import SE3
global control_loop_return
control_loop_return = tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]
def PathFollowingFromPlannerCtrllLoopTemplate(
- path_planner: ProcessManager|FunctionType,
+ path_planner: ProcessManager | FunctionType,
get_position: Callable[[AbstractRobotManager], np.ndarray],
SOLVER: Any,
control_loop: Callable[
[
Any,
- np.ndarray,
+ np.ndarray | list[SE3],
Namespace,
AbstractRobotManager,
int,
@@ -59,6 +60,8 @@ def PathFollowingFromPlannerCtrllLoopTemplate(
_, path2D = data
path2D = np.array(path2D).reshape((-1, 2))
else:
+ # NOTE: DOES NOT HAVE TO BE 2D IN THIS CASE,
+ # TODO: RENAME APROPRIATELY
path2D = path_planner(p)
if len(path2D) < 4:
breakFlag = True
diff --git a/python/smc/control/optimal_control/abstract_croco_ocp.py b/python/smc/control/optimal_control/abstract_croco_ocp.py
index 356a068..94d2ff9 100644
--- a/python/smc/control/optimal_control/abstract_croco_ocp.py
+++ b/python/smc/control/optimal_control/abstract_croco_ocp.py
@@ -55,6 +55,7 @@ class CrocoOCP(abc.ABC):
self.constructRegulationCosts()
self.constructStateLimits()
self.constructConstraints()
+ self.constructTaskCostsValues()
self.constructTaskObjectiveFunction(goal)
self.constructRunningModels()
self.problem = crocoddyl.ShootingProblem(
@@ -220,6 +221,9 @@ class CrocoOCP(abc.ABC):
0.0,
)
+ @abc.abstractmethod
+ def constructTaskCostsValues(self) -> None: ...
+
@abc.abstractmethod
def constructTaskObjectiveFunction(self, goal) -> None: ...
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 6a98cdc..73844d3 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,3 @@
-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, DualArmWholeBodyInterface
from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
@@ -64,7 +63,7 @@ def CrocoBasePathFollowingMPC(
robot: MobileBaseInterface,
x0: np.ndarray,
path_planner: ProcessManager | types.FunctionType,
-) -> None:
+run=True) -> None|ControlLoopManager:
"""
CrocoBasePathFollowingMPC
-----
@@ -93,9 +92,13 @@ def CrocoBasePathFollowingMPC(
loop_manager = ControlLoopManager(
robot, controlLoop, args, save_past_item, log_item
)
- loop_manager.run()
+ if run:
+ loop_manager.run()
+ else:
+ return loop_manager
+# NOTE: DEPRICATED WILL GET DELETED SOON (TEMPLATE HANDLES IT NOW)
def CrocoEEPathFollowingMPCControlLoop(
args: Namespace,
robot: SingleArmInterface,
@@ -190,7 +193,7 @@ def CrocoEEPathFollowingMPC(
robot: SingleArmInterface,
x0: np.ndarray,
path_planner: ProcessManager | types.FunctionType,
-) -> None:
+run=True) -> None|ControlLoopManager:
"""
CrocoEndEffectorPathFollowingMPC
-----
@@ -207,6 +210,7 @@ def CrocoEEPathFollowingMPC(
if type(path_planner) == types.FunctionType:
controlLoop = partial(
+ # NOTE: DEPRICATED WILL GET DELETED SOON (TEMPLATE HANDLES IT NOW)
CrocoEEPathFollowingMPCControlLoop, args, robot, ocp, path_planner
)
else:
@@ -229,9 +233,12 @@ def CrocoEEPathFollowingMPC(
loop_manager = ControlLoopManager(
robot, controlLoop, args, save_past_item, log_item
)
- loop_manager.run()
-
+ if run:
+ loop_manager.run()
+ else:
+ return loop_manager
+# NOTE: DEPRICATED WILL GET DELETED SOON (TEMPLATE HANDLES IT NOW)
def BaseAndEEPathFollowingMPCControlLoop(
args: Namespace,
robot,
@@ -332,7 +339,7 @@ def BaseAndEEPathFollowingMPC(
args: Namespace,
robot: SingleArmWholeBodyInterface,
path_planner: ProcessManager | types.FunctionType,
-) -> None:
+run=True) -> None|ControlLoopManager:
"""
BaseAndEEPathFollowingMPC
-----
@@ -354,6 +361,7 @@ def BaseAndEEPathFollowingMPC(
if type(path_planner) == types.FunctionType:
controlLoop = partial(
+# NOTE: DEPRICATED WILL GET DELETED SOON (TEMPLATE HANDLES IT NOW)
BaseAndEEPathFollowingMPCControlLoop, args, robot, ocp, path_planner
)
else:
@@ -385,7 +393,10 @@ def BaseAndEEPathFollowingMPC(
path2D_handlebar[i] for i in range(args.past_window_size)
)
- loop_manager.run()
+ if run:
+ loop_manager.run()
+ else:
+ return loop_manager
# TODO: the robot put in is a whole body robot,
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 99ed117..64a13fd 100644
--- a/python/smc/control/optimal_control/path_following_croco_ocp.py
+++ b/python/smc/control/optimal_control/path_following_croco_ocp.py
@@ -1,4 +1,10 @@
-from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
+from smc.control.optimal_control.point_to_point_croco_ocp import (
+ BaseIKOCP,
+ SingleArmIKOCP,
+ DualArmIKOCP,
+ BaseAndSingleArmIKOCP,
+ BaseAndDualArmIKOCP,
+)
from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
from smc.robots.interfaces.single_arm_interface import SingleArmInterface
from smc.robots.interfaces.dual_arm_interface import DualArmInterface
@@ -8,11 +14,13 @@ from smc.robots.interfaces.whole_body_interface import (
)
import numpy as np
-import crocoddyl
from argparse import Namespace
-class BasePathFollowingOCP(CrocoOCP):
+# NOTE: all of these should be rewritten so that it inherits from BaseIKOCP,
+# and then you just override update goal.
+# you'd cut this code in less than half
+class BasePathFollowingOCP(BaseIKOCP):
"""
createBaseAndEEPathFollowingOCP
-------------------------------
@@ -22,31 +30,9 @@ class BasePathFollowingOCP(CrocoOCP):
"""
def __init__(self, args, robot: MobileBaseInterface, x0):
- goal = None
+ goal = robot.T_w_b.translation.copy()
super().__init__(args, robot, x0, goal)
- def constructTaskObjectiveFunction(self, goal) -> None:
- path_base = [np.append(self.x0[:2], 0.0)] * self.args.n_knots
-
- for i in range(self.args.n_knots):
- baseTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
- self.state, self.robot.base_frame_id, path_base[i], self.state.nv
- )
- baseTrackingCost = crocoddyl.CostModelResidual(
- self.state, baseTranslationResidual
- )
- self.runningCostModels[i].addCost(
- "base_translation" + str(i),
- baseTrackingCost,
- self.args.base_translation_cost,
- )
-
- self.terminalCostModel.addCost(
- "base_translation" + str(self.args.n_knots),
- baseTrackingCost,
- self.args.base_translation_cost,
- )
-
def updateCosts(self, data):
path_base = data
for i, runningModel in enumerate(self.solver.problem.runningModels):
@@ -60,7 +46,7 @@ class BasePathFollowingOCP(CrocoOCP):
].cost.residual.reference = path_base[-1]
-class CrocoEEPathFollowingOCP(CrocoOCP):
+class CrocoEEPathFollowingOCP(SingleArmIKOCP):
"""
createCrocoEEPathFollowingOCP
-------------------------------
@@ -70,34 +56,9 @@ class CrocoEEPathFollowingOCP(CrocoOCP):
"""
def __init__(self, args: Namespace, robot: SingleArmInterface, x0: np.ndarray):
- goal = None
+ goal = robot.T_w_e
super().__init__(args, robot, x0, goal)
- def constructTaskObjectiveFunction(self, goal):
- T_w_e = self.robot.T_w_e
- path = [T_w_e] * self.args.n_knots
-
- for i in range(self.args.n_knots):
- framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
- self.state,
- self.robot.ee_frame_id,
- path[i], # this better be done with the same time steps as the knots
- # NOTE: this should be done outside of this function to clarity
- self.state.nv,
- )
- goalTrackingCost = crocoddyl.CostModelResidual(
- self.state, framePlacementResidual
- )
- self.runningCostModels[i].addCost(
- "gripperPose" + str(i), goalTrackingCost, 1e2
- )
- # runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
-
- self.terminalCostModel.addCost(
- "gripperPose" + str(self.args.n_knots), goalTrackingCost, 1e2
- )
- # terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
-
def updateCosts(self, data):
for i, runningModel in enumerate(self.solver.problem.runningModels):
runningModel.differential.costs.costs[
@@ -109,7 +70,7 @@ class CrocoEEPathFollowingOCP(CrocoOCP):
].cost.residual.reference = data[-1]
-class BaseAndEEPathFollowingOCP(CrocoOCP):
+class BaseAndEEPathFollowingOCP(BaseAndSingleArmIKOCP):
"""
createBaseAndEEPathFollowingOCP
-------------------------------
@@ -119,48 +80,9 @@ class BaseAndEEPathFollowingOCP(CrocoOCP):
"""
def __init__(self, args, robot: SingleArmWholeBodyInterface, x0):
- goal = None
+ goal = (robot.T_w_e, robot.T_w_b.translation.copy)
super().__init__(args, robot, x0, goal)
- def constructTaskObjectiveFunction(self, goal) -> None:
- T_w_e = self.robot.T_w_e
- path_ee = [T_w_e] * self.args.n_knots
- path_base = [np.append(self.x0[:2], 0.0)] * self.args.n_knots
-
- for i in range(self.args.n_knots):
- eePoseResidual = crocoddyl.ResidualModelFramePlacement(
- self.state,
- self.robot.ee_frame_id,
- path_ee[i], # this better be done with the same time steps as the knots
- # NOTE: this should be done outside of this function to clarity
- self.state.nv,
- )
- eeTrackingCost = crocoddyl.CostModelResidual(self.state, eePoseResidual)
- self.runningCostModels[i].addCost(
- "ee_pose" + str(i), eeTrackingCost, self.args.ee_pose_cost
- )
-
- baseTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
- self.state, self.robot.base_frame_id, path_base[i], self.state.nv
- )
- baseTrackingCost = crocoddyl.CostModelResidual(
- self.state, baseTranslationResidual
- )
- self.runningCostModels[i].addCost(
- "base_translation" + str(i),
- baseTrackingCost,
- self.args.base_translation_cost,
- )
-
- self.terminalCostModel.addCost(
- "ee_pose" + str(self.args.n_knots), eeTrackingCost, self.args.ee_pose_cost
- )
- self.terminalCostModel.addCost(
- "base_translation" + str(self.args.n_knots),
- baseTrackingCost,
- self.args.base_translation_cost,
- )
-
def updateCosts(self, data):
path_base = data[0]
pathSE3 = data[1]
@@ -181,71 +103,34 @@ class BaseAndEEPathFollowingOCP(CrocoOCP):
].cost.residual.reference = pathSE3[-1]
-class DualArmEEPathFollowingOCP(CrocoOCP):
+class DualArmEEPathFollowingOCP(DualArmIKOCP):
def __init__(self, args, robot: DualArmInterface, x0):
- goal = None
- raise NotImplementedError
+ goal = (robot.T_w_l, robot.T_w_r)
super().__init__(args, robot, x0, goal)
+ def updateCosts(self, data):
+ pathSE3_l = data[0]
+ pathSE3_r = data[1]
+ for i, runningModel in enumerate(self.solver.problem.runningModels):
+ runningModel.differential.costs.costs[
+ "gripperPose_l" + str(i)
+ ].cost.residual.reference = pathSE3_l[i]
+ runningModel.differential.costs.costs[
+ "gripperPose_r" + str(i)
+ ].cost.residual.reference = pathSE3_r[i]
-class BaseAndDualArmEEPathFollowingOCP(CrocoOCP):
- def __init__(self, args, robot: DualArmWholeBodyInterface, x0):
- goal = None
- super().__init__(args, robot, x0, goal)
+ self.solver.problem.terminalModel.differential.costs.costs[
+ "gripperPose_l" + str(self.args.n_knots)
+ ].cost.residual.reference = pathSE3_l[-1]
+ self.solver.problem.terminalModel.differential.costs.costs[
+ "gripperPose_r" + str(self.args.n_knots)
+ ].cost.residual.reference = pathSE3_r[-1]
- def constructTaskObjectiveFunction(self, goal) -> None:
- 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
- for i in range(self.args.n_knots):
- l_eePoseResidual = crocoddyl.ResidualModelFramePlacement(
- self.state,
- self.robot.l_ee_frame_id,
- path_ee_left[i],
- self.state.nv,
- )
- l_eeTrackingCost = crocoddyl.CostModelResidual(self.state, l_eePoseResidual)
- self.runningCostModels[i].addCost(
- "l_ee_pose" + str(i), l_eeTrackingCost, self.args.ee_pose_cost
- )
- r_eePoseResidual = crocoddyl.ResidualModelFramePlacement(
- self.state,
- self.robot.r_ee_frame_id,
- path_ee_right[i],
- self.state.nv,
- )
- r_eeTrackingCost = crocoddyl.CostModelResidual(self.state, r_eePoseResidual)
- self.runningCostModels[i].addCost(
- "r_ee_pose" + str(i), r_eeTrackingCost, self.args.ee_pose_cost
- )
- baseTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
- self.state, self.robot.base_frame_id, path_base[i], self.state.nv
- )
- baseTrackingCost = crocoddyl.CostModelResidual(
- self.state, baseTranslationResidual
- )
- self.runningCostModels[i].addCost(
- "base_translation" + str(i),
- baseTrackingCost,
- self.args.base_translation_cost,
- )
- self.terminalCostModel.addCost(
- "l_ee_pose" + str(self.args.n_knots),
- l_eeTrackingCost,
- self.args.ee_pose_cost,
- )
- self.terminalCostModel.addCost(
- "r_ee_pose" + str(self.args.n_knots),
- r_eeTrackingCost,
- self.args.ee_pose_cost,
- )
- self.terminalCostModel.addCost(
- "base_translation" + str(self.args.n_knots),
- baseTrackingCost,
- self.args.base_translation_cost,
- )
+class BaseAndDualArmEEPathFollowingOCP(BaseAndDualArmIKOCP):
+ def __init__(self, args, robot: DualArmWholeBodyInterface, x0):
+ goal = robot.T_w_l, robot.T_w_r, robot.T_w_b.translation
+ super().__init__(args, robot, x0, goal)
def updateCosts(self, data):
path_base = data[0]
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 5fec443..616956d 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
@@ -11,6 +11,7 @@ import pinocchio as pin
import crocoddyl
from argparse import Namespace
+
class BaseIKOCP(CrocoOCP):
def __init__(
self,
@@ -21,6 +22,14 @@ class BaseIKOCP(CrocoOCP):
):
super().__init__(args, robot, x0, p_basegoal)
+ def constructTaskCostsValues(self):
+ self.base_translation_cost_values = np.linspace(
+ self.args.base_translation_cost,
+ self.args.base_translation_cost
+ * self.args.linearly_increasing_task_cost_factor,
+ self.args.n_knots + 1,
+ )
+
def constructTaskObjectiveFunction(self, goal) -> None:
p_basegoal = goal
for i in range(self.args.n_knots):
@@ -33,12 +42,12 @@ class BaseIKOCP(CrocoOCP):
self.runningCostModels[i].addCost(
"base_translation" + str(i),
baseTrackingCost,
- self.args.base_translation_cost,
+ self.base_translation_cost_values[i],
)
self.terminalCostModel.addCost(
"base_translation" + str(self.args.n_knots),
baseTrackingCost,
- self.args.base_translation_cost,
+ self.base_translation_cost_values[-1],
)
# there is nothing to update in a point-to-point task
@@ -56,7 +65,6 @@ class BaseIKOCP(CrocoOCP):
].cost.residual.reference = p_basegoal
-
class SingleArmIKOCP(CrocoOCP):
def __init__(
self,
@@ -67,6 +75,13 @@ class SingleArmIKOCP(CrocoOCP):
):
super().__init__(args, robot, x0, T_w_goal)
+ def constructTaskCostsValues(self):
+ self.ee_pose_cost_values = np.linspace(
+ self.args.ee_pose_cost,
+ self.args.ee_pose_cost * self.args.linearly_increasing_task_cost_factor,
+ self.args.n_knots + 1,
+ )
+
def constructTaskObjectiveFunction(self, goal) -> None:
T_w_goal = goal
framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
@@ -91,10 +106,12 @@ class SingleArmIKOCP(CrocoOCP):
# )
for i in range(self.args.n_knots):
self.runningCostModels[i].addCost(
- "gripperPose" + str(i), goalTrackingCost, 1e2
+ "gripperPose" + str(i), goalTrackingCost, self.ee_pose_cost_values[i]
)
self.terminalCostModel.addCost(
- "gripperPose" + str(self.args.n_knots), goalTrackingCost, 1e2
+ "gripperPose" + str(self.args.n_knots),
+ goalTrackingCost,
+ self.ee_pose_cost_values[-1],
)
# self.terminalCostModel.addCost("velFinal", frameVelocityCost, 1e3)
@@ -119,6 +136,13 @@ class DualArmIKOCP(CrocoOCP):
):
super().__init__(args, robot, x0, goal)
+ def constructTaskCostsValues(self):
+ self.ee_pose_cost_values = np.linspace(
+ self.args.ee_pose_cost,
+ self.args.ee_pose_cost * self.args.linearly_increasing_task_cost_factor,
+ self.args.n_knots + 1,
+ )
+
def constructTaskObjectiveFunction(self, goal) -> None:
T_w_lgoal, T_w_rgoal = goal
framePlacementResidual_l = crocoddyl.ResidualModelFramePlacement(
@@ -162,16 +186,24 @@ class DualArmIKOCP(CrocoOCP):
# )
for i in range(self.args.n_knots):
self.runningCostModels[i].addCost(
- "gripperPose_l" + str(i), goalTrackingCost_l, 1e2
+ "gripperPose_l" + str(i),
+ goalTrackingCost_l,
+ self.ee_pose_cost_values[i],
)
self.runningCostModels[i].addCost(
- "gripperPose_r" + str(i), goalTrackingCost_r, 1e2
+ "gripperPose_r" + str(i),
+ goalTrackingCost_r,
+ self.ee_pose_cost_values[i],
)
self.terminalCostModel.addCost(
- "gripperPose_l" + str(self.args.n_knots), goalTrackingCost_l, 1e2
+ "gripperPose_l" + str(self.args.n_knots),
+ goalTrackingCost_l,
+ self.ee_pose_cost_values[-1],
)
self.terminalCostModel.addCost(
- "gripperPose_r" + str(self.args.n_knots), goalTrackingCost_r, 1e2
+ "gripperPose_r" + str(self.args.n_knots),
+ goalTrackingCost_r,
+ self.ee_pose_cost_values[-1],
)
# self.terminalCostModel.addCost("velFinal_l", frameVelocityCost_l, 1e3)
# self.terminalCostModel.addCost("velFinal_r", frameVelocityCost_r, 1e3)
@@ -180,7 +212,8 @@ class DualArmIKOCP(CrocoOCP):
def updateCosts(self, data):
pass
- def updateGoalInModels(self, T_w_lgoal: pin.SE3, T_w_rgoal: pin.SE3) -> None:
+ def updateGoalInModels(self, goal) -> None:
+ T_w_lgoal, T_w_rgoal = goal
for i, runningModel in enumerate(self.solver.problem.runningModels):
runningModel.differential.costs.costs[
"gripperPose_l" + str(i)
@@ -207,6 +240,19 @@ class BaseAndSingleArmIKOCP(SingleArmIKOCP, BaseIKOCP):
):
super().__init__(args, robot, x0, goal)
+ def constructTaskCostsValues(self):
+ self.base_translation_cost_values = np.linspace(
+ self.args.base_translation_cost,
+ self.args.base_translation_cost
+ * self.args.linearly_increasing_task_cost_factor,
+ self.args.n_knots + 1,
+ )
+ self.ee_pose_cost_values = np.linspace(
+ self.args.ee_pose_cost,
+ self.args.ee_pose_cost * self.args.linearly_increasing_task_cost_factor,
+ self.args.n_knots + 1,
+ )
+
def constructTaskObjectiveFunction(
self,
goal,
@@ -236,6 +282,19 @@ class BaseAndDualArmIKOCP(DualArmIKOCP, BaseIKOCP):
):
super().__init__(args, robot, x0, goal)
+ def constructTaskCostsValues(self):
+ self.base_translation_cost_values = np.linspace(
+ self.args.base_translation_cost,
+ self.args.base_translation_cost
+ * self.args.linearly_increasing_task_cost_factor,
+ self.args.n_knots + 1,
+ )
+ self.ee_pose_cost_values = np.linspace(
+ self.args.ee_pose_cost,
+ self.args.ee_pose_cost * self.args.linearly_increasing_task_cost_factor,
+ self.args.n_knots + 1,
+ )
+
def constructTaskObjectiveFunction(
self,
goal,
@@ -250,5 +309,5 @@ class BaseAndDualArmIKOCP(DualArmIKOCP, BaseIKOCP):
def updateGoalInModels(self, goal) -> None:
T_w_lgoal, T_w_rgoal, p_basegoal = goal
- super().updateGoalInModels(T_w_lgoal, T_w_rgoal)
+ super().updateGoalInModels((T_w_lgoal, T_w_rgoal))
BaseIKOCP.updateGoalInModels(self, p_basegoal)
diff --git a/python/smc/control/optimal_control/util.py b/python/smc/control/optimal_control/util.py
index 16b220b..703fd41 100644
--- a/python/smc/control/optimal_control/util.py
+++ b/python/smc/control/optimal_control/util.py
@@ -16,9 +16,9 @@ def get_OCP_args(parser: argparse.ArgumentParser):
)
parser.add_argument(
"--stop-at-final",
- type=int,
- default=1,
- help="the trajectory generated by the OCP will be followed. it might not have finally velocity 0. \
+ action=argparse.BooleanOptionalAction,
+ default=False,
+ help="NOTE: NOT IMPLEMENTED ATM the trajectory generated by the OCP will be followed. it might not have finally velocity 0. \
if this argument is set to true, the final velocity will be set to 0 (there will be overshoot).",
)
parser.add_argument(
@@ -49,4 +49,10 @@ def get_OCP_args(parser: argparse.ArgumentParser):
parser.add_argument(
"--u-reg-cost", type=float, default=1e-3, help="control input regulation cost"
)
+ parser.add_argument(
+ "--linearly-increasing-task-cost-factor",
+ type=float,
+ default=1.0,
+ help="the first knot gets task cost you set with --task-cost, after which it's linearly increased until the last knot has this argument * --task-cost",
+ )
return parser
diff --git a/python/smc/robots/interfaces/mobile_base_interface.py b/python/smc/robots/interfaces/mobile_base_interface.py
index 022502b..6a266da 100644
--- a/python/smc/robots/interfaces/mobile_base_interface.py
+++ b/python/smc/robots/interfaces/mobile_base_interface.py
@@ -47,6 +47,11 @@ class MobileBaseInterface(AbstractRobotManager):
def T_w_b(self):
return self._T_w_b.copy()
+ # NOTE: lil bit of evil to run some algorithms
+ @property
+ def T_w_e(self):
+ return self._T_w_b.copy()
+
def computeT_w_b(self, q: np.ndarray) -> pin.SE3:
assert type(q) is np.ndarray
pin.forwardKinematics(
@@ -77,6 +82,12 @@ class MobileBaseInterface(AbstractRobotManager):
self._updateV()
self.forwardKinematics()
+ def getJacobian(self) -> np.ndarray:
+ J = pin.computeFrameJacobian(
+ self.model, self.data, self._q, self._base_frame_id
+ )
+ return J
+
def get_mobile_base_model(underactuated: bool) -> tuple[pin.Model, pin.GeometryModel]:
--
GitLab