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 280cb399b0a07d0c8ae7b00c41eb41093ce50661..5b8b6772fc7b77626568d717644030f52d336654 100644
--- a/python/smc/control/cartesian_space/cartesian_space_trajectory_following.py
+++ b/python/smc/control/cartesian_space/cartesian_space_trajectory_following.py
@@ -1,5 +1,6 @@
from smc.control.control_loop_manager import ControlLoopManager
from smc.multiprocessing.process_manager import ProcessManager
+from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
from smc.robots.interfaces.single_arm_interface import SingleArmInterface
from smc.control.controller_templates.path_following_template import (
PathFollowingFromPlannerCtrllLoopTemplate,
@@ -8,6 +9,7 @@ from smc.control.cartesian_space.ik_solvers import getIKSolver, dampedPseudoinve
from smc.path_generation.path_math.path2d_to_6d import (
path2D_to_SE3,
)
+from smc.path_generation.path_math.path_to_trajectory import path2D_to_trajectory2D
from functools import partial
import pinocchio as pin
@@ -19,9 +21,9 @@ import types
def cartesianPathFollowingControlLoop(
+ rot_x: float,
ik_solver: Callable[[np.ndarray, np.ndarray], np.ndarray],
path: list[pin.SE3] | np.ndarray,
- rot_x: float,
args: Namespace,
robot: SingleArmInterface,
t: int,
@@ -35,12 +37,19 @@ def cartesianPathFollowingControlLoop(
# TODO: refactor this horror out of here
if type(path) == np.ndarray:
+ # TODO: this would be cool but i can't unfortunatelly
+ # velocity = args.max_v_percentage
+ # traj = path2D_to_trajectory2D(args, path, velocity)
+ # path = path2D_to_SE3(traj[:, :2], 0.0, rot_x)
path = path2D_to_SE3(path[:, :2], 0.0, rot_x)
# 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
+ err_vector = pin.log6(SEerror).vector
+ if np.linalg.norm(err_vector) < 0.2:
+ V_path = 5 * pin.log6(path[1].actInv(path[2])).vector
+ err_vector += V_path
+ err_vector[3:] = err_vector[3:] * 2
J = robot.getJacobian()
v_cmd = ik_solver(J, err_vector)
@@ -60,29 +69,35 @@ def cartesianPathFollowingControlLoop(
if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
robot.visualizer_manager.sendCommand({"frame_path": path[:20]})
- return v_cmd, {}, {}
+ return (
+ v_cmd,
+ {},
+ {"err_vec_ee": err_vector},
+ )
def cartesianPathFollowingWithPlanner(
args: Namespace,
robot: SingleArmInterface,
path_planner: ProcessManager | types.FunctionType,
+ x_rot: float,
run=True,
) -> None | ControlLoopManager:
ik_solver = getIKSolver(args, robot)
get_position = lambda robot: robot.T_w_e.translation[:2]
+ loop = partial(cartesianPathFollowingControlLoop, x_rot)
controlLoop = partial(
PathFollowingFromPlannerCtrllLoopTemplate,
path_planner,
get_position,
ik_solver,
- cartesianPathFollowingControlLoop,
+ loop,
args,
robot,
)
log_item = {
- "qs": np.zeros(robot.model.nq),
- "dqs": np.zeros(robot.model.nv),
+ "qs": np.zeros(robot.nq),
+ "dqs": np.zeros(robot.nv),
"err_vec_ee": np.zeros(6),
}
save_past_item = {}
diff --git a/python/smc/path_generation/path_math/path2d_to_6d.py b/python/smc/path_generation/path_math/path2d_to_6d.py
index 5adcce9c23af09c9dd597eb6017b00b8da4da7bc..64d893de48c666ee05bdd11896f6808b5d3d05e7 100644
--- a/python/smc/path_generation/path_math/path2d_to_6d.py
+++ b/python/smc/path_generation/path_math/path2d_to_6d.py
@@ -25,8 +25,8 @@ def path2D_to_SE3(
y_diff = y_i_plus_1 - y_i
# elementwise arctan2
# should be y first, then x
- # thetas = np.arctan2(y_diff, x_diff)
- thetas = np.arctan2(x_diff, y_diff)
+ thetas = np.arctan2(y_diff, x_diff)
+ # thetas = np.arctan2(x_diff, y_diff)
######################################
# construct SE3 from SE2 reference #
@@ -44,15 +44,16 @@ def path2D_to_SE3(
# [0.0, 0.0, -1.0],
# ]
# )
- rotation = np.array(
- [
- [np.cos(thetas[i]), -np.sin(thetas[i]), 0.0],
- [np.sin(thetas[i]), np.cos(thetas[i]), 0.0],
- [0.0, 0.0, 1.0],
- ]
- )
- rot_mat_x = pin.rpy.rpyToMaxtrix(rox_t, 0.0, 0.0)
- rotation = rot_mat_x @ rotation
+ # rotation = np.array(
+ # [
+ # [np.cos(thetas[i]), -np.sin(thetas[i]), 0.0],
+ # [np.sin(thetas[i]), np.cos(thetas[i]), 0.0],
+ # [0.0, 0.0, 1.0],
+ # ]
+ # )
+ rotation = pin.rpy.rpyToMatrix(0.0, 0.0, thetas[i])
+ rot_mat_x = pin.rpy.rpyToMatrix(rot_x, 0.0, 0.0)
+ rotation = rotation @ rot_mat_x
# rotation = pin.rpy.rpyToMatrix(0.0, 0.0, thetas[i])
# rotation = pin.rpy.rpyToMatrix(np.pi / 2, np.pi / 2, 0.0) @ rotation
translation = np.array([path2D[i][0], path2D[i][1], path_height])
diff --git a/python/smc/robots/interfaces/whole_body_dual_arm_interface.py b/python/smc/robots/interfaces/whole_body_dual_arm_interface.py
index 9ab517bf074d6fb56ae5fc550ffdaca47bad654c..231a852274ad6fcdb2ced62a0bde4686d4cee926 100644
--- a/python/smc/robots/interfaces/whole_body_dual_arm_interface.py
+++ b/python/smc/robots/interfaces/whole_body_dual_arm_interface.py
@@ -166,14 +166,13 @@ class DualArmWholeBodyInterface(DualArmInterface, MobileBaseInterface):
if self._mode == self.control_mode.left_arm_only:
return J_left_with_base[:, 3:]
- # NOTE: the base jacobian can be extracted from either left or right frame -
- # since it's a body jacobian both have to be the same at the base.
- # for efficiency of course it would be best to construct it in place,
- # but who cares if it runs on time either way.
+ # NOTE: for some reason this is necessary as the J_left_with_base does not produce what i want
+ # ALSO, the assumption is that the error is given in the body frame (like all others)
if self._mode == self.control_mode.base_only:
J_base = np.zeros((6, 3))
- J_base[:2, :2] = self.T_w_b.rotation[:2, :2]
- J_base[5, 2] = 1
+ J_base[0, 0] = 1.0
+ J_base[1, 1] = 1.0
+ J_base[5, 2] = 1.0
return J_base
# return J_left_with_base[:, :3]