From 8def0648614b40d7e4864d48390f91214fefea75 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Thu, 27 Feb 2025 15:40:04 +0100
Subject: [PATCH] moveLDualArm works. not sure if this is the final API form
for dual armed robots, but i'm quite happy with what's here now. next up is
wholebodyyumi. a todo for the future is to package urdf-related files (stls
etc) for yumi.
---
.../technical_report/cart_pulling.log | 2 +-
.../technical_report/cart_pulling.pdf | Bin 288786 -> 288786 bytes
examples/cart_pulling/todos.toml | 69 ++++-
examples/cartesian_space/dual_arm_clik.py | 50 ++++
.../cartesian_space_point_to_point.py | 257 ++++++------------
.../smc/control/cartesian_space/ik_solvers.py | 10 +-
python/smc/control/control_loop_manager.py | 18 +-
.../croco_mpc_point_to_point.py | 16 +-
python/smc/robots/implementations/__init__.py | 1 +
python/smc/robots/implementations/heron.py | 5 +-
python/smc/robots/implementations/yumi.py | 24 +-
.../robots/interfaces/dual_arm_interface.py | 76 +++++-
python/smc/robots/utils.py | 20 +-
13 files changed, 327 insertions(+), 221 deletions(-)
create mode 100644 examples/cartesian_space/dual_arm_clik.py
diff --git a/examples/cart_pulling/technical_report/cart_pulling.log b/examples/cart_pulling/technical_report/cart_pulling.log
index c94d615..15cc14d 100644
--- a/examples/cart_pulling/technical_report/cart_pulling.log
+++ b/examples/cart_pulling/technical_report/cart_pulling.log
@@ -1,4 +1,4 @@
-This is pdfTeX, Version 3.141592653-2.6-1.40.26 (TeX Live 2024/Arch Linux) (preloaded format=pdflatex 2025.2.22) 26 FEB 2025 10:30
+This is pdfTeX, Version 3.141592653-2.6-1.40.26 (TeX Live 2024/Arch Linux) (preloaded format=pdflatex 2025.2.22) 26 FEB 2025 11:47
entering extended mode
\write18 enabled.
%&-line parsing enabled.
diff --git a/examples/cart_pulling/technical_report/cart_pulling.pdf b/examples/cart_pulling/technical_report/cart_pulling.pdf
index de7c9151127cbd4e479a974680060eb7de0f65c5..9c4ffafbdc6ff237ebdc0e4f72fd896447bdadc1 100644
GIT binary patch
delta 114
zcmbPqL2%Lq!G;#b7N!>FEi4}cSPf0gjVz{r3t*9hFt<wvvb+{?wlp$yG&grOG%;{8
kbT)D}ay55xb~Z9_GIw&cG<R_}u~V=iq-48$8p~-$0B`di-T(jq
delta 114
zcmbPqL2%Lq!G;#b7N!>FEi4}cSPhH~j0~oK3t*9hFt<wvvb+{?HZ?P{FmSSPHgGaF
kF)}uEbTe>uG<P#~Gca{Aa&t6uvs17kq-48$8p~-$0A8OSq5uE@
diff --git a/examples/cart_pulling/todos.toml b/examples/cart_pulling/todos.toml
index bc1f789..6712381 100644
--- a/examples/cart_pulling/todos.toml
+++ b/examples/cart_pulling/todos.toml
@@ -27,6 +27,7 @@ is_done = true
dependencies = []
purpose = """being able moving heron and yumi with code"""
hours_required = 0
+assignee = "Marko"
[[project_plan.action_items.action_items]]
@@ -44,6 +45,7 @@ is_done = true
dependencies = []
purpose = """verify heron interface to be able to rest assuredly"""
hours_required = 2
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "exploring & understanding whole-body-driver"
@@ -62,6 +64,7 @@ is_done = false
dependencies = []
purpose = """being able to move mobile yumi with code"""
hours_required = 12
+assignee = "Marko"
[[project_plan.action_items.action_items]]
@@ -77,6 +80,7 @@ is_done = false
dependencies = ["exploring & understanding whole-body-driver"]
purpose = """being able to move mobile yumi with something other than whole body driver"""
hours_required = 7
+assignee = "Marko"
[[project_plan.action_items.action_items]]
@@ -92,6 +96,7 @@ purpose = """making sure we are getting the best control performance in the sens
of frequency, also ensures the approach isn't fundamentally bad/incorrect (ROS2 qos
profiles etc)"""
hours_required = 4
+assignee = "Marko"
[[project_plan.action_items]]
name = "fast localization"
@@ -105,6 +110,7 @@ dependencies = []
purpose = """localization is required for correctness of any kind of mobile robot movement,
especially for whole-body control"""
hours_required = 0
+assignee = "Seba"
[[project_plan.action_items.action_items]]
@@ -123,6 +129,7 @@ dependencies = []
purpose = """we need some kind of odometry for fast localization, and it's better to use the provided
thing over wasting time hacking a sub-par solution from various topics"""
hours_required = 6
+assignee = "Seba"
[[project_plan.action_items.action_items]]
name = "testing whether raw odometry + amcl is enough"
@@ -135,6 +142,7 @@ is_done = false
dependencies = ["understanding and testing odometry"]
purpose = """trying to avoid the work of using a kalman filter"""
hours_required = 3
+assignee = "Seba"
[[project_plan.action_items.action_items]]
@@ -151,6 +159,7 @@ is_done = false
dependencies = ["testing whether raw odometry + amcl is enough"]
purpose = """configuring a kf if necessary"""
hours_required = 6
+assignee = "Seba"
[[project_plan.action_items]]
name = "ros2 whatever map into path planning map"
@@ -173,6 +182,7 @@ dependencies = []
purpose = """localization is required for correctness of any kind of mobile robot movement,
especially for whole-body control"""
hours_required = 0
+assignee = "Seba"
[[project_plan.action_items.action_items]]
@@ -189,6 +199,7 @@ is_done = false
dependencies = []
purpose = """no map no planning, need to get it from nav2"""
hours_required = 4
+assignee = "Seba"
[[project_plan.action_items.action_items]]
name = "understanding starifiable obstacles"
@@ -201,6 +212,7 @@ is_done = false
dependencies = []
purpose = """no map no planning, need to get it from nav2"""
hours_required = 4
+assignee = "Seba"
[[project_plan.action_items.action_items]]
name = "starify static map"
@@ -213,6 +225,7 @@ is_done = false
dependencies = []
purpose = """no map no planning, need to get it from nav2"""
hours_required = 3
+assignee = "Seba"
[[project_plan.action_items.action_items]]
name = "starify dynamic obstacles"
@@ -225,6 +238,7 @@ is_done = false
dependencies = []
purpose = """no map no planning, need to get it from nav2"""
hours_required = 3
+assignee = "Seba"
[[project_plan.action_items.action_items]]
name = "continuously starify dynamic obstacles"
@@ -237,6 +251,7 @@ is_done = false
dependencies = []
purpose = """no map no planning, need to get it from nav2"""
hours_required = 5
+assignee = "Seba"
[[project_plan.action_items]]
name = "computer vision stuff"
@@ -256,6 +271,7 @@ dependencies = []
purpose = """required to solve the real problem of a robot pulling a real cart -
it has to see it. also easily usable in other projects if done right codewise."""
hours_required = 0
+assignee = "X"
[[project_plan.action_items.action_items]]
@@ -269,6 +285,7 @@ is_done = false
dependencies = []
purpose = """needed for yolo cross-validation"""
hours_required = 2
+assignee = "X"
[[project_plan.action_items.action_items]]
@@ -282,6 +299,7 @@ is_done = false
dependencies = []
purpose = """required for cross-validation with yolo"""
hours_required = 4
+assignee = "X"
[[project_plan.action_items.action_items]]
name = "fix pose estimation from point cloud"
@@ -296,6 +314,7 @@ is_done = false
dependencies = []
purpose = """last component of core vision stack, required for anything vision"""
hours_required = 10
+assignee = "X"
[[project_plan.action_items.action_items]]
name = "finding a local server to run grounding sam on"
@@ -310,6 +329,7 @@ purpose = """
grounding sam is a very good off-the-shelf solution for computer vision tasks. this should be available literally at any point in time to enable quick experiments
"""
hours_required = 3
+assignee = "X"
[[project_plan.action_items]]
name = "refactor optimal control code"
@@ -324,6 +344,7 @@ purpose = """
making optimal control fixes possible
"""
hours_required = 0
+assignee = "Marko"
[[project_plan.action_items.action_items]]
@@ -339,6 +360,7 @@ purpose = """
at least 100 less lines of copy-pasted code
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "refactor p2p ocp"
@@ -353,6 +375,7 @@ purpose = """
making optimal control fixes possible
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "refactor end-effector path following"
@@ -367,6 +390,7 @@ purpose = """
making optimal control fixes possible
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "rewrite ee construction from past path to be more efficient"
@@ -383,6 +407,7 @@ purpose = """
making optimal control fixes possible
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "write end-effector and base path following"
@@ -408,13 +433,14 @@ control stack is broken. could be the control itself, or trajectories' construct
or there is something fundamentally fucked with changing the plan on the fly, idk. could be that you need to put in some smoothing into it. can't know unless you write tools to test this shit.
"""
priority = 1
-is_done = false
+is_done = true
deadline = 2025-02-14
dependencies = ["refactor optimal control code"]
purpose = """
getting any base and ee path tracking to work
"""
hours_required = 0
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "manage path saving better"
@@ -436,6 +462,7 @@ purpose = """
getting any base and ee path tracking to work
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items.action_items]]
@@ -451,6 +478,7 @@ purpose = """
getting any base and ee path tracking to work
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "ensure either reference doesn't change rapidly"
@@ -471,6 +499,7 @@ purpose = """
getting any base and ee path tracking to work
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "debug base and ee tracking"
@@ -481,13 +510,14 @@ do just path reconstruction. it should be a live controlloop.p_base should go ar
PLOT EVERY SINGLE STEP! make the plan providing be on a single processor.
"""
priority = 1
-is_done = false
+is_done = true
deadline = 2025-02-14
dependencies = ["dual arm feature", "refactor end-effector path following"]
purpose = """
solving the same problem but with a single arm
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "alternative if full-body mpc is not the right tool for the job"
@@ -496,13 +526,14 @@ control just the base with mpc for base tracking.
make the handlebar be defined in a pose relative to the body. run clik on arms to get to a point made up by the path (still need to find corresponding path point) with the orientation being constructed with normal/tangent of the past path. then run p2p on that single goal. update the goal at every time step. an even better idea if getting whole trajectory works, just run trajectory following.
"""
priority = 1
-is_done = false
+is_done = true
deadline = 2025-02-14
dependencies = ["dual arm feature", "refactor end-effector path following"]
purpose = """
half of paper problem
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "dual arm p2p and path following"
@@ -517,6 +548,7 @@ purpose = """
half of paper problem
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "cart-pulling (ee and base path following)"
@@ -531,6 +563,7 @@ purpose = """
half of paper problem
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "construct map showcasing my prefered setting (make the labyrinth!!!!!!!!)"
@@ -545,6 +578,7 @@ purpose = """
"""
hours_required = 3
+assignee = "Marko"
[[project_plan.action_items]]
name = "tools for trajectory generation in smc"
@@ -564,6 +598,7 @@ purpose = """
this tool will solve issues with weird trajectories or wasting 20min to write the correct SE3 transformation, which simply persist otherwise and are super annoying to deal with -> it's much better to do geometry on screen than in your head if you need specific numbers on that geometry.
"""
hours_required = 0
+assignee = "Marko"
[[project_plan.action_items.action_items]]
@@ -572,11 +607,12 @@ description = """
debug existing trajectory generation
"""
priority = 1
-is_done = false
+is_done = true
deadline = 2025-02-14
dependencies = ["smc refactor"]
purpose = """creating the base of interactive goal/transform/trajectory design"""
hours_required = 8
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "extend logging to include saving the whole of save-past-dict"
@@ -597,6 +633,7 @@ deadline = 2025-02-14
dependencies = ["smc refactor"]
purpose = """creating the base of interactive goal/transform/trajectory design"""
hours_required = 8
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "use drawing feature from drawing demo to draw a path to be followed"
@@ -611,6 +648,7 @@ deadline = 2025-02-14
dependencies = []
purpose = """make test design for just path planning take 10 seconds"""
hours_required = 10
+assignee = "Marko"
[[project_plan.action_items.action_items]]
name = "interactive meshcat for setting a transformation"
@@ -627,6 +665,7 @@ deadline = 2025-02-14
dependencies = ["smc refactor"]
purpose = """creating the base of interactive goal/transform/trajectory design"""
hours_required = 10
+assignee = "Marko"
[[project_plan.action_items.action_items]]
@@ -643,6 +682,7 @@ deadline = 2025-02-14
dependencies = ["interactive meshcat for setting a transformation"]
purpose = """making interaction much more usable"""
hours_required = 2
+assignee = "Marko"
[[project_plan.action_items.action_items]]
@@ -658,6 +698,7 @@ deadline = 2025-02-14
dependencies = ["interactive meshcat for setting a transformation"]
purpose = """making visual verification more powerful and easier"""
hours_required = 4
+assignee = "Marko"
[[project_plan.action_items.action_items]]
@@ -673,6 +714,7 @@ deadline = 2025-02-01
dependencies = ["interactive meshcat for setting a transformation"]
purpose = """making tuning and understanding of path planning much easier"""
hours_required = 6
+assignee = "Marko"
[[project_plan.action_items]]
@@ -690,6 +732,7 @@ deadline = 2025-02-01
dependencies = ["tools for trajectory generation in smc"]
purpose = """having the correct ocp formulation for the whole-body mpc controller for pushing and pulling"""
hours_required = 20
+assignee = "Marko"
[[project_plan.action_items]]
@@ -704,6 +747,7 @@ deadline = 2025-02-01
dependencies = []
purpose = """designing a better ocp, testing agaist different options"""
hours_required = 12
+assignee = "Marko"
[[project_plan.action_items]]
@@ -736,6 +780,7 @@ deadline = 2025-02-01
dependencies = []
purpose = """having functional robotic system, not such 1 controller hopefully working as intended"""
hours_required = 8
+assignee = "Marko"
[[project_plan.action_items]]
@@ -763,6 +808,7 @@ purpose = """
scientific perfomance measurement is required for a scientific publication
"""
hours_required = 0
+assignee = "Marko"
[[project_plan.action_items.action_items]]
@@ -779,6 +825,7 @@ is_done = false
dependencies = []
purpose = """knowing what to measure and why"""
hours_required = 12
+assignee = "all"
[[project_plan.action_items.action_items]]
@@ -802,6 +849,7 @@ is_done = false
dependencies = []
purpose = """getting data"""
hours_required = 18
+assignee = "Marko"
[[project_plan.action_items]]
name = "write research diary"
@@ -812,7 +860,7 @@ this text is also not the paper text. the paper text is a condensed subsection o
"""
priority = 1
deadline = ["iros paper submission deadline", 2025-03-01]
-is_done = false
+is_done = true
dependencies = []
purpose = """this is the product of the project"""
hours_required = 6
@@ -832,7 +880,7 @@ is_done = false
dependencies = []
purpose = """this is the product of the project"""
hours_required = 0
-assignee = "Seba"
+assignee = "Marko, Seba"
[[project_plan.action_items.action_items]]
name = "setting colors to authors"
@@ -934,6 +982,7 @@ is_done = false
dependencies = []
purpose = """part of paper"""
hours_required = 12
+assignee = "Seba"
[[project_plan.action_items.action_items]]
name = "writing related work"
@@ -948,6 +997,7 @@ is_done = false
dependencies = []
purpose = """part of paper"""
hours_required = 12
+assignee = "Seba"
[[project_plan.action_items.action_items]]
name = "writing methodology"
@@ -962,6 +1012,7 @@ is_done = false
dependencies = []
purpose = """part of paper"""
hours_required = 12
+assignee = "Marko, Seba"
[[project_plan.action_items.action_items.action_items]]
name = "writing modelling"
@@ -974,6 +1025,7 @@ is_done = false
dependencies = []
purpose = """part of paper"""
hours_required = 12
+assignee = "Seba"
[[project_plan.action_items.action_items.action_items]]
name = "writing CV, how to grab a cart, small behavior tree mabe"
@@ -986,6 +1038,7 @@ is_done = false
dependencies = []
purpose = """part of paper"""
hours_required = 12
+assignee = "X"
[[project_plan.action_items.action_items.action_items]]
name = "writing path planning"
@@ -998,6 +1051,7 @@ is_done = false
dependencies = []
purpose = """part of paper"""
hours_required = 12
+assignee = "Seba"
[[project_plan.action_items.action_items.action_items]]
name = "writing controller"
@@ -1010,6 +1064,7 @@ is_done = false
dependencies = []
purpose = """part of paper"""
hours_required = 12
+assignee = "Marko"
[[project_plan.action_items.action_items]]
@@ -1025,6 +1080,7 @@ is_done = false
dependencies = ["performing experiments"]
purpose = """part of paper"""
hours_required = 18
+assignee = "Marko"
@@ -1042,3 +1098,4 @@ dependencies = ["writing introdution, related work, methodology",
"writing experiments, discussion"]
purpose = """part of paper"""
hours_required = 24
+assignee = "all"
diff --git a/examples/cartesian_space/dual_arm_clik.py b/examples/cartesian_space/dual_arm_clik.py
new file mode 100644
index 0000000..b9cfc1f
--- /dev/null
+++ b/examples/cartesian_space/dual_arm_clik.py
@@ -0,0 +1,50 @@
+from smc import getMinimalArgParser, getRobotFromArgs
+from smc.util.define_random_goal import getRandomlyGeneratedGoal
+from smc.control.cartesian_space import getClikArgs
+from smc.robots.utils import defineGoalPointCLI
+from smc.control.cartesian_space.cartesian_space_point_to_point import moveLDualArm
+
+import numpy as np
+import argparse
+import pinocchio as pin
+
+
+def get_args():
+ parser = getMinimalArgParser()
+ parser.description = "Run closed loop inverse kinematics \
+ of various kinds. Make sure you know what the goal is before you run!"
+ parser = getClikArgs(parser)
+ parser.add_argument(
+ "--randomly-generate-goal",
+ action=argparse.BooleanOptionalAction,
+ default=False,
+ help="if true, rand generate a goal, if false you type it in via text input",
+ )
+ args = parser.parse_args()
+ return args
+
+
+if __name__ == "__main__":
+ args = get_args()
+ robot = getRobotFromArgs(args)
+ if args.randomly_generate_goal:
+ T_w_goal = getRandomlyGeneratedGoal(args)
+ if args.visualizer:
+ robot.visualizer_manager.sendCommand({"Mgoal": T_w_goal})
+ else:
+ T_w_goal = defineGoalPointCLI(robot)
+ T_w_goal.rotation = np.eye(3)
+ T_absgoal_lgoal = pin.SE3.Identity()
+ T_absgoal_lgoal.translation[1] = 0.1
+ T_absgoal_rgoal = pin.SE3.Identity()
+ T_absgoal_rgoal.translation[1] = -0.1
+ moveLDualArm(args, robot, T_w_goal, T_absgoal_lgoal, T_absgoal_rgoal)
+
+ if args.real:
+ robot.stopRobot()
+
+ if args.visualizer:
+ robot.killManipulatorVisualizer()
+
+ if args.save_log:
+ robot._log_manager.saveLog()
diff --git a/python/smc/control/cartesian_space/cartesian_space_point_to_point.py b/python/smc/control/cartesian_space/cartesian_space_point_to_point.py
index 945c100..f9b373a 100644
--- a/python/smc/control/cartesian_space/cartesian_space_point_to_point.py
+++ b/python/smc/control/cartesian_space/cartesian_space_point_to_point.py
@@ -1,5 +1,7 @@
+from IPython import embed
from smc.control.control_loop_manager import ControlLoopManager
from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+from smc.robots.interfaces.dual_arm_interface import DualArmInterface
from smc.robots.interfaces.force_torque_sensor_interface import (
ForceTorqueOnSingleArmWrist,
)
@@ -8,9 +10,19 @@ from functools import partial
import pinocchio as pin
import numpy as np
from argparse import Namespace
+from collections import deque
+from typing import Callable
-def controlLoopClik(robot: SingleArmInterface, ik_solver, T_w_goal: pin.SE3, _, __):
+def controlLoopClik(
+ args: Namespace,
+ robot: SingleArmInterface,
+ # J err_vec v_cmd
+ ik_solver: Callable[[np.ndarray, np.ndarray], np.ndarray],
+ T_w_goal: pin.SE3,
+ i: int,
+ past_data: dict[str, deque[np.ndarray]],
+) -> tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]:
"""
controlLoopClik
---------------
@@ -26,7 +38,7 @@ def controlLoopClik(robot: SingleArmInterface, ik_solver, T_w_goal: pin.SE3, _,
# first check whether we're at the goal
SEerror = T_w_e.actInv(T_w_goal)
err_vector = pin.log6(SEerror).vector
- if np.linalg.norm(err_vector) < robot.args.goal_error:
+ if np.linalg.norm(err_vector) < args.goal_error:
breakFlag = True
J = robot.getJacobian()
# compute the joint velocities based on controller you passed
@@ -47,7 +59,7 @@ def controlLoopClik(robot: SingleArmInterface, ik_solver, T_w_goal: pin.SE3, _,
return breakFlag, save_past_item, log_item
-def moveL(args: Namespace, robot: SingleArmInterface, T_w_goal):
+def moveL(args: Namespace, robot: SingleArmInterface, T_w_goal: pin.SE3) -> None:
"""
moveL
-----
@@ -75,7 +87,9 @@ def moveL(args: Namespace, robot: SingleArmInterface, T_w_goal):
# TODO: implement
-def moveLFollowingLine(args: Namespace, robot, goal_point):
+def moveLFollowingLine(
+ args: Namespace, robot, goal_point: pin.SE3
+) -> tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]:
"""
moveLFollowingLine
------------------
@@ -87,17 +101,18 @@ def moveLFollowingLine(args: Namespace, robot, goal_point):
then another version goes in a line and linearly updates the orientation
as it goes
"""
- pass
+ ...
def moveUntilContactControlLoop(
args: Namespace,
robot: ForceTorqueOnSingleArmWrist,
speed: np.ndarray,
- ik_solver,
- i,
- past_data,
-):
+ # J err_vec v_cmd
+ ik_solver: Callable[[np.ndarray, np.ndarray], np.ndarray],
+ i: int,
+ past_data: dict[str, deque[np.ndarray]],
+) -> tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]:
"""
moveUntilContactControlLoop
---------------
@@ -158,26 +173,32 @@ def moveUntilContact(
print("Collision detected!!")
-# NOTE: this shit makes no sense, it's old and broken
-# but you will want to look at it for dual arm stuff so i'm leaving it here for the time being
-"""
-def controlLoopClikDualArmsOnly(
- robot: AbstractRobotManager, ik_solver, goal_transform, i, past_data
-):
-
- controlLoopClikDualArmsOnly
+def controlLoopClikDualArm(
+ args: Namespace,
+ robot: DualArmInterface,
+ # J err_vec v_cmd
+ ik_solver: Callable[[np.ndarray, np.ndarray], np.ndarray],
+ T_w_absgoal: pin.SE3,
+ T_abs_lgoal: pin.SE3,
+ T_abs_rgoal: pin.SE3,
+ i: int,
+ past_data: dict[str, deque[np.ndarray]],
+) -> tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]:
+ """
+ controlLoopClikDualArm
---------------
+ do point to point motion for each arm and its goal.
+ that goal is generated from a single goal that you pass,
+ and an SE3 transformation on the goal for each arm
+ """
breakFlag = False
log_item = {}
- q = robot.q
- T_w_e_left, T_w_e_right = robot.getT_w_e()
- #
- Mgoal_left = robot.Mgoal.act(goal_transform)
- Mgoal_right = robot.Mgoal.act(goal_transform.inverse())
+ T_w_lgoal = T_abs_lgoal.act(T_w_absgoal)
+ T_w_rgoal = T_abs_rgoal.act(T_w_absgoal)
- SEerror_left = T_w_e_left.actInv(Mgoal_left)
- SEerror_right = T_w_e_right.actInv(Mgoal_right)
+ SEerror_left = robot.T_w_l.actInv(T_w_lgoal)
+ SEerror_right = robot.T_w_r.actInv(T_w_rgoal)
err_vector_left = pin.log6(SEerror_left).vector
err_vector_right = pin.log6(SEerror_right).vector
@@ -186,174 +207,54 @@ def controlLoopClikDualArmsOnly(
np.linalg.norm(err_vector_right) < robot.args.goal_error
):
breakFlag = True
- J_left = pin.computeFrameJacobian(robot.model, robot.data, q, robot.l_ee_frame_id)
- J_left = J_left[:, 3:]
- J_right = pin.computeFrameJacobian(robot.model, robot.data, q, robot.r_ee_frame_id)
- J_right = J_right[:, 3:]
-
+ err_vector = np.concatenate((err_vector_left, err_vector_right))
+ J = robot.getJacobian()
# compute the joint velocities based on controller you passed
- qd_left = ik_solver(J_left, err_vector_left)
- qd_right = ik_solver(J_right, err_vector_right)
- # lb, ub = (-0.05 * robot.model.robot.model.velocityLimit, 0.05 * robot.model.robot.model.velocityLimit)
- # qd_left = invKinmQP(J_left, err_vector_left, lb=lb[3:], ub=ub[3:])
- # qd_right = invKinmQP(J_right, err_vector_right, lb=lb[3:], ub=ub[3:])
- qd = qd_left + qd_right
- qd = np.hstack((np.zeros(3), qd))
- robot.sendVelocityCommand(qd)
+ # this works exactly the way you think it does:
+ # the velocities for the other arm are 0
+ # what happens to the base hasn't been investigated but it seems ok
+ v_cmd = ik_solver(J, err_vector)
+ robot.sendVelocityCommand(v_cmd)
- log_item["qs"] = q.reshape((robot.model.nq,))
- log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
- log_item["dqs_cmd"] = qd.reshape((robot.model.nv,))
- # we're not saving here, but need to respect the API,
- # hence the empty dict
+ log_item["qs"] = robot.q
+ log_item["dqs"] = robot.v
+ log_item["dqs_cmd"] = v_cmd
return breakFlag, {}, log_item
-"""
-# NOTE: this shit makes no sense, it's old and broken
-# but you will want to look at it for dual arm stuff so i'm leaving it here for the time being
-"""
-def moveLDualArmsOnly(
- args, robot: AbstractRobotManager, goal_point, goal_transform, run=True
-):
- ===
- moveL
- -----
- does moveL.
- send a SE3 object as goal point.
- if you don't care about rotation, make it np.zeros((3,3))
- ===
- # assert type(goal_point) == pin.pinocchio_pywrap.SE3
+def moveLDualArm(
+ args: Namespace,
+ robot: DualArmInterface,
+ T_w_goal: pin.SE3,
+ T_abs_l: pin.SE3,
+ T_abs_r: pin.SE3,
+ run=True,
+) -> None:
+ """
+ moveLDualArm
+ -----------
+ """
+ # TODO: clean this up
+ robot._mode = DualArmInterface.control_mode.both
ik_solver = getIKSolver(args, robot)
- controlLoop = partial(controlLoopClikDualArmsOnly, robot, ik_solver, goal_transform)
+ controlLoop = partial(
+ controlLoopClikDualArm, args, robot, ik_solver, T_w_goal, T_abs_l, T_abs_r
+ )
# we're not using any past data or logging, hence the empty arguments
log_item = {
- "qs": np.zeros(robot.model.nq),
- "dqs": np.zeros(robot.model.nv),
- "dqs_cmd": np.zeros(robot.model.nv),
+ "qs": np.zeros(robot.nq),
+ "dqs": np.zeros(robot.nv),
+ "dqs_cmd": np.zeros(robot.nv),
}
- loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
+ save_past_dict = {}
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_dict, log_item
+ )
if run:
loop_manager.run()
else:
return loop_manager
-"""
-# NOTE: this shit makes no sense, it's old and broken
-# but you will want to look at it for dual arm stuff so i'm leaving it here for the time being
-"""
-def controlLoopClikDualArm(
- robot: DualArmInterface, ik_solver, goal_transform, i, past_data
-):
-# controlLoopClikDualArm
-# ---------------
-# do point to point motion for each arm and its goal.
-# that goal is generated from a single goal that you pass,
-# and a transformation on the goal you also pass.
-# the transformation is done in goal's frame (goal is and has
-# to be an SE3 object).
-# the transform is designed for the left arm,
-# and its inverse is applied for the right arm.
-
-
-
-
- breakFlag = False
- log_item = {}
- q = robot.q
- T_w_e_left, T_w_e_right = robot.getT_w_e()
- #
- Mgoal_left = goal_transform.act(robot.Mgoal)
- Mgoal_right = goal_transform.inverse().act(robot.Mgoal)
- # print("robot.Mgoal", robot.Mgoal)
- # print("Mgoal_left", Mgoal_left)
- # print("Mgoal_right", Mgoal_right)
-
- SEerror_left = T_w_e_left.actInv(Mgoal_left)
- SEerror_right = T_w_e_right.actInv(Mgoal_right)
-
- err_vector_left = pin.log6(SEerror_left).vector
- err_vector_right = pin.log6(SEerror_right).vector
-
- if (np.linalg.norm(err_vector_left) < robot.args.goal_error) and (
- np.linalg.norm(err_vector_right) < robot.args.goal_error
- ):
- breakFlag = True
- J_left = pin.computeFrameJacobian(robot.model, robot.data, q, robot.l_ee_frame_id)
- J_right = pin.computeFrameJacobian(robot.model, robot.data, q, robot.r_ee_frame_id)
- # compute the joint velocities based on controller you passed
- # this works exactly the way you think it does:
- # the velocities for the other arm are 0
- # what happens to the base hasn't been investigated but it seems ok
- qd_left = ik_solver(J_left, err_vector_left)
- qd_right = ik_solver(J_right, err_vector_right)
- qd = qd_left + qd_right
- robot.sendVelocityCommand(qd)
-
- log_item["qs"] = q.reshape((robot.model.nq,))
- log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
- log_item["dqs_cmd"] = qd.reshape((robot.model.nv,))
- # we're not saving here, but need to respect the API,
- # hence the empty dict
- return breakFlag, {}, log_item
-"""
-
-
-# NOTE: this shit makes no sense, it's old and broken
-# but you will want to look at it for dual arm stuff so i'm leaving it here for the time being
-"""
-def clikDualArm(args, robot, goal, goal_transform, run=True):
-#
-# clikDualArm
-# -----------
-#
- robot.Mgoal = copy.deepcopy(goal)
- ik_solver = getClikController(args, robot)
- controlLoop = partial(controlLoopClikDualArm, robot, ik_solver, goal_transform)
- # we're not using any past data or logging, hence the empty arguments
- log_item = {
- 'qs' : np.zeros(robot.model.nq),
- 'dqs' : np.zeros(robot.model.nv),
- 'dqs_cmd' : np.zeros(robot.model.nv),
- }
- save_past_dict = {}
- loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
- if run:
- loop_manager.run()
- else:
- return loop_manager
-"""
-
-
-# NOTE: this shit makes no sense, it's old and broken
-# but you will want to look at it for dual arm stuff so i'm leaving it here for the time being
-"""
-def controlLoopClikArmOnly(robot, ik_solver, _, _):
- breakFlag = False
- log_item = {}
- q = robot.q
- T_w_e = robot.getT_w_e()
- # first check whether we're at the goal
- SEerror = T_w_e.actInv(robot.Mgoal)
- err_vector = pin.log6(SEerror).vector
- if np.linalg.norm(err_vector) < robot.args.goal_error:
- breakFlag = True
- J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
- # cut off base from jacobian
- J = J[:, 3:]
- # compute the joint velocities based on controller you passed
- qd = ik_solver(J, err_vector)
- # add the missing velocities back
- qd = np.hstack((np.zeros(3), qd))
- robot.sendVelocityCommand(qd)
-
- log_item["qs"] = q.reshape((robot.model.nq,))
- log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
- log_item["dqs_cmd"] = qd.reshape((robot.model.nv,))
- # we're not saving here, but need to respect the API,
- # hence the empty dict
- return breakFlag, {}, log_item
-"""
# NOTE: this shit makes no sense, it's old and broken
# but you will want to look at it for dual arm stuff so i'm leaving it here for the time being
diff --git a/python/smc/control/cartesian_space/ik_solvers.py b/python/smc/control/cartesian_space/ik_solvers.py
index f517d9b..01f7244 100644
--- a/python/smc/control/cartesian_space/ik_solvers.py
+++ b/python/smc/control/cartesian_space/ik_solvers.py
@@ -1,10 +1,18 @@
+from typing import Callable
import numpy as np
from qpsolvers import solve_qp
import proxsuite
+
from functools import partial
+from argparse import Namespace
+
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+from smc.robots.interfaces.dual_arm_interface import DualArmInterface
-def getIKSolver(args, robot):
+def getIKSolver(
+ args: Namespace, robot: SingleArmInterface | DualArmInterface
+) -> Callable[[np.ndarray, np.ndarray], np.ndarray]:
"""
getIKSolver
-----------------
diff --git a/python/smc/control/control_loop_manager.py b/python/smc/control/control_loop_manager.py
index 6b92aab..9951ff1 100644
--- a/python/smc/control/control_loop_manager.py
+++ b/python/smc/control/control_loop_manager.py
@@ -149,17 +149,11 @@ class ControlLoopManager:
"q": self.robot_manager._q,
}
)
- if self.robot_manager.robot_name != "yumi":
- self.robot_manager.visualizer_manager.sendCommand(
- {
- "T_w_e": self.robot_manager.T_w_e,
- }
- )
- else:
- T_w_e_left, T_w_e_right = self.robot_manager.T_w_e
- self.robot_manager.visualizer_manager.sendCommand(
- {"T_w_e": T_w_e_left}
- )
+ self.robot_manager.visualizer_manager.sendCommand(
+ {
+ "T_w_e": self.robot_manager.T_w_e,
+ }
+ )
if self.robot_manager.robot_name == "heron":
T_base = self.robot_manager.data.oMi[1]
self.robot_manager.visualizer_manager.sendCommand(
@@ -170,7 +164,7 @@ class ControlLoopManager:
# self.robot_manager.visualizer_manager.sendCommand({"T_base" : T_base})
if self.args.visualize_collision_approximation:
- raise NotADirectoryError
+ raise NotImplementedError
# TODO: here call robot manager's update ellipses function
if self.args.plotter:
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 3f8ce0e..0e0aeb6 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
@@ -1,3 +1,4 @@
+from argparse import Namespace
from smc.robots.interfaces.single_arm_interface import SingleArmInterface
from smc.control.control_loop_manager import ControlLoopManager
from smc.control.optimal_control.point_to_point_croco_ocp import (
@@ -10,7 +11,14 @@ import numpy as np
from functools import partial
-def CrocoEEP2PMPCControlLoop(args, robot: SingleArmInterface, ocp, T_w_goal, _, __):
+def CrocoEEP2PMPCControlLoop(
+ args: Namespace,
+ robot: SingleArmInterface,
+ ocp,
+ T_w_goal: pin.SE3,
+ _: int,
+ __: dict[str, deque[np.ndarray]],
+):
"""
CrocoIKMPCControlLoop
---------------------
@@ -42,7 +50,7 @@ def CrocoEEP2PMPCControlLoop(args, robot: SingleArmInterface, ocp, T_w_goal, _,
return breakFlag, save_past_dict, log_item
-def EEP2PMPC(args, robot, T_w_goal, run=True):
+def EEP2PMPC(args: Namespace, robot, T_w_goal: pin.SE3, run=True):
"""
IKMPC
-----
@@ -79,8 +87,8 @@ def CrocoP2PEEAndBaseMPCControlLoop(
ocp,
T_w_eegoal: pin.SE3,
p_basegoal: np.ndarray,
- _,
- __,
+ _: int,
+ __: dict[str, deque[np.ndarray]],
):
"""
CrocoP2PEEAndBaseMPCControlLoop
diff --git a/python/smc/robots/implementations/__init__.py b/python/smc/robots/implementations/__init__.py
index 6a9cc72..98afce5 100644
--- a/python/smc/robots/implementations/__init__.py
+++ b/python/smc/robots/implementations/__init__.py
@@ -1,6 +1,7 @@
# from .heron import RobotManagerHeron, SimulatedHeronRobotManager
from .heron import *
from .mir import *
+from .yumi import *
from .mobile_yumi import *
from .ur5e import RobotManagerUR5e, SimulatedUR5eRobotManager
from importlib.util import find_spec
diff --git a/python/smc/robots/implementations/heron.py b/python/smc/robots/implementations/heron.py
index b4a9edb..45ef20f 100644
--- a/python/smc/robots/implementations/heron.py
+++ b/python/smc/robots/implementations/heron.py
@@ -9,6 +9,7 @@ from smc.robots.interfaces.mobile_base_interface import (
from smc.robots.interfaces.whole_body_interface import SingleArmWholeBodyInterface
from smc.robots.implementations.ur5e import get_model
+from argparse import Namespace
import time
import numpy as np
import pinocchio as pin
@@ -50,7 +51,7 @@ class RobotManagerHeron(
class SimulatedHeronRobotManager(AbstractSimulatedRobotManager, RobotManagerHeron):
- def __init__(self, args):
+ def __init__(self, args: Namespace):
if args.debug_prints:
print("SimulatedRobotManagerHeron init")
super().__init__(args)
@@ -58,7 +59,7 @@ class SimulatedHeronRobotManager(AbstractSimulatedRobotManager, RobotManagerHero
# NOTE: overriding wrench stuff here
# there can be a debated whether there should be a simulated forcetorquesensorinterface,
# but it's annoying as hell and there is no immediate benefit in solving this problem
- def _updateWrench(self):
+ def _updateWrench(self) -> None:
self._wrench_base = np.random.random(6)
# NOTE: create a robot_math module, make this mapping a function called
# mapse3ToDifferent frame or something like that
diff --git a/python/smc/robots/implementations/yumi.py b/python/smc/robots/implementations/yumi.py
index b96ac69..be195f2 100644
--- a/python/smc/robots/implementations/yumi.py
+++ b/python/smc/robots/implementations/yumi.py
@@ -1,11 +1,13 @@
+from smc.robots.abstract_simulated_robot import AbstractSimulatedRobotManager
from smc.robots.interfaces.dual_arm_interface import DualArmInterface
+from argparse import Namespace
from importlib.resources import files
from os import path
import pinocchio as pin
-class RobotManagerYuMi(DualArmInterface):
+class AbstractYuMiRobotManager(DualArmInterface):
@property
def model(self):
@@ -25,11 +27,25 @@ class RobotManagerYuMi(DualArmInterface):
def __init__(self, args):
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")
+ print("RobotManagerYuMi init")
+ self._model, self._collision_model, self._visual_model, self._data = (
+ get_yumi_model()
+ )
+ self._l_ee_frame_id = self.model.getFrameId("robl_tool0")
+ self._r_ee_frame_id = self.model.getFrameId("robr_tool0")
self._MAX_ACCELERATION = 1.7 # const
self._MAX_QD = 3.14 # const
+
+ self._mode: DualArmInterface.control_mode = DualArmInterface.control_mode.both
+ super().__init__(args)
+
+
+class SimulatedYuMiRobotManager(
+ AbstractSimulatedRobotManager, AbstractYuMiRobotManager
+):
+ def __init__(self, args: Namespace):
+ if args.debug_prints:
+ print("SimulatedYuMiRobotManager init")
super().__init__(args)
diff --git a/python/smc/robots/interfaces/dual_arm_interface.py b/python/smc/robots/interfaces/dual_arm_interface.py
index f93465e..468ec8d 100644
--- a/python/smc/robots/interfaces/dual_arm_interface.py
+++ b/python/smc/robots/interfaces/dual_arm_interface.py
@@ -4,6 +4,7 @@ import abc
import numpy as np
import pinocchio as pin
from argparse import Namespace
+from enum import Enum
class DualArmInterface(SingleArmInterface):
@@ -15,6 +16,8 @@ class DualArmInterface(SingleArmInterface):
- r stands for right
"""
+ control_mode = Enum("mode", [("both", 3), ("left", 4), ("right", 5)])
+
# NOTE: you need to fill in the specific names from the urdf here for your robot
# (better than putting in a magic number)
# self._l_ee_frame_id = self.model.getFrameId("left_ee_name")
@@ -31,6 +34,58 @@ class DualArmInterface(SingleArmInterface):
print("DualArmInterface init")
super().__init__(args)
+ @property
+ def mode(self) -> control_mode:
+ return self._mode
+
+ @mode.setter
+ def setMode(self, mode: control_mode) -> None:
+ assert type(mode) is self.control_mode
+ self._mode = mode
+
+ @property
+ def q(self) -> np.ndarray:
+ # NOTE: left should be on the left side of the joint values array
+ if self._mode.value == 4:
+ return self._q[: self.model.nq // 2]
+
+ # NOTE: right should be on the left side of the joint values array
+ if self._mode.value == 5:
+ return self._q[self.model.nq // 2 :]
+
+ return self._q.copy()
+
+ @property
+ def nq(self):
+ if self._mode.value == 4:
+ return self.model.nq // 2
+
+ if self._mode.value == 5:
+ return self.model.nq // 2
+
+ return self.model.nq
+
+ @property
+ def v(self) -> np.ndarray:
+ if self._mode.value == 4:
+ return self._v[: self.model.nv // 2]
+
+ if self._mode.value == 5:
+ return self._v[: self.model.nv // 2]
+
+ return self._v.copy()
+
+ @property
+ def nv(self):
+ if self._mode.value == 4:
+ return self.model.nv // 2
+
+ if self._mode.value == 5:
+ return self.model.nv // 2
+
+ return self.model.nv
+
+ # NOTE: might be misleading and unhelpful
@property
def T_w_e(self):
return self.T_w_abs
@@ -54,11 +109,12 @@ class DualArmInterface(SingleArmInterface):
# T_abs_l and T_abs_r are relative transformations between the absolute dual-arm frame,
# and the left and right end-effector frames respectively
@property
- @abc.abstractmethod
- def T_abs_r(self) -> pin.SE3: ...
+ def T_abs_r(self) -> pin.SE3:
+ return self._T_w_abs.actInv(self._T_w_r)
+
@property
- @abc.abstractmethod
- def T_abs_l(self) -> pin.SE3: ...
+ def T_abs_l(self) -> pin.SE3:
+ return self._T_w_abs.actInv(self._T_w_l)
@property
def T_w_abs(self) -> pin.SE3:
@@ -133,12 +189,16 @@ class DualArmInterface(SingleArmInterface):
# the other arm filled with zeros
J_left = pin.computeFrameJacobian(
self.model, self.data, self._q, self._l_ee_frame_id
- )
+ )[:, : self.model.nq // 2]
+ if self._mode.value == 4:
+ return J_left
J_right = pin.computeFrameJacobian(
self.model, self.data, self._q, self._r_ee_frame_id
- )
+ )[:, self.model.nq // 2 :]
+ if self._mode.value == 5:
+ return J_right
J = np.zeros((12, self.nq))
- J[: self.nq, :6] = J_left
- J[self.nq :, 6:] = J_right
+ J[:6, : self.model.nq // 2] = J_left
+ J[6:, self.model.nq // 2 :] = J_right
return J
diff --git a/python/smc/robots/utils.py b/python/smc/robots/utils.py
index 6f83f34..f55a561 100644
--- a/python/smc/robots/utils.py
+++ b/python/smc/robots/utils.py
@@ -19,15 +19,15 @@ def getMinimalArgParser():
Simple Manipulator Control",
formatter_class=argparse.ArgumentDefaultsHelpFormatter,
)
- #################################################
- # general arguments: connection, plotting etc #
- #################################################
+ ########################################################
+ # general arguments: robot, simulation, 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"],
+ choices=["ur5e", "heron", "yumi", "mobile_yumi"],
)
parser.add_argument(
"--real",
@@ -209,7 +209,7 @@ def defineGoalPointCLI(robot):
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)
+ *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
@@ -260,6 +260,16 @@ def getRobotFromArgs(args: argparse.Namespace) -> AbstractRobotManager:
# return RealHeronRobotManager(args)
else:
return SimulatedHeronRobotManager(args)
+ if args.robot == "yumi":
+ if args.real:
+ pass
+ # TODO: finish it
+ # return RealYuMiRobotManager(args)
+ else:
+ return SimulatedYuMiRobotManager(args)
+ raise NotImplementedError(
+ f"robot {args.robot} is not supported! run the script you ran with --help to see what's available"
+ )
# if args.robot == "mir":
--
GitLab