From a62153c6c4f4678d018f99053f30b7741541a02f Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Tue, 11 Mar 2025 01:54:55 +0100
Subject: [PATCH] done 4 today
---
.../control_sub_problems/dual_arm_cart_pulling.py | 12 ++++++++----
.../disjoint_control/mpc_base_clik_single_arm.py | 2 +-
.../path_math/cart_pulling_path_math.py | 2 +-
3 files changed, 10 insertions(+), 6 deletions(-)
diff --git a/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling.py b/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling.py
index 1cabaa0..8414bc0 100644
--- a/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling.py
+++ b/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling.py
@@ -72,7 +72,10 @@ if __name__ == "__main__":
################################################
# 2. construct initial T_w_abs (starting middle of handlebar pose)
################################################
- pathSE3 = path2D_to_SE3(path_base, args.handlebar_height)
+ pathSE3 = path2D_to_SE3(path_base, args.handlebar_height, np.pi)
+ init_rot = pathSE3[0].rotation @ pin.rpy.rpyToMatrix(np.pi, 0.0, 0.0)
+ init_rot = init_rot @ pin.rpy.rpyToMatrix(0.0, 0.0, np.pi)
+ # T_w_absgoal = constructInitialT_w_abs(args, path_base, init_rot)
T_w_absgoal = constructInitialT_w_abs(args, path_base, pathSE3[0].rotation)
if args.visualizer:
@@ -105,9 +108,10 @@ if __name__ == "__main__":
robot.mode = robot.control_mode.base_only
# NOTE: T_w_absgoal's x-axis points toward the path.
# z-axis points down by an undocumented choice
- rotation_init_rpy = pin.rpy.matrixToRpy(T_w_absgoal.rotation)
- rotation_init = pin.rpy.rpyToMatrix(0.0, 0.0, rotation_init_rpy[2] + np.pi / 2)
- T_w_bgoal = pin.SE3(rotation_init, p_basegoal)
+ # rotation_init_rpy = pin.rpy.matrixToRpy(T_w_absgoal.rotation)
+ # rotation_init = pin.rpy.rpyToMatrix(0.0, 0.0, rotation_init_rpy[2] - np.pi / 2)
+ # T_w_bgoal = pin.SE3(rotation_init, p_basegoal)
+ T_w_bgoal = pin.SE3(init_rot, p_basegoal)
moveL(args, robot, T_w_bgoal)
###################################################
diff --git a/examples/cart_pulling/disjoint_control/mpc_base_clik_single_arm.py b/examples/cart_pulling/disjoint_control/mpc_base_clik_single_arm.py
index 41a1f7c..8379bcb 100644
--- a/examples/cart_pulling/disjoint_control/mpc_base_clik_single_arm.py
+++ b/examples/cart_pulling/disjoint_control/mpc_base_clik_single_arm.py
@@ -74,7 +74,7 @@ if __name__ == "__main__":
_, path2D = data
path2D = np.array(path2D).reshape((-1, 2))
- pathSE3 = path2D_to_SE3(path2D, args.handlebar_height)
+ pathSE3 = path2D_to_SE3(path2D, args.handlebar_height, np.pi)
if args.visualizer:
# TODO: document this somewhere
robot.visualizer_manager.sendCommand({"Mgoal": pathSE3[0]})
diff --git a/python/smc/path_generation/path_math/cart_pulling_path_math.py b/python/smc/path_generation/path_math/cart_pulling_path_math.py
index 1a1e54d..a8b8977 100644
--- a/python/smc/path_generation/path_math/cart_pulling_path_math.py
+++ b/python/smc/path_generation/path_math/cart_pulling_path_math.py
@@ -123,5 +123,5 @@ def construct_EE_path(
)
# step (4)
- ee_SE3trajectory = path2D_to_SE3(ee_2Dtrajectory, args.handlebar_height)
+ ee_SE3trajectory = path2D_to_SE3(ee_2Dtrajectory, args.handlebar_height, np.pi)
return ee_SE3trajectory
--
GitLab