From ec147c9df609ac8079337eca0fe2a922b91716e9 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Tue, 11 Mar 2025 12:07:13 +0100
Subject: [PATCH] can navigate mobile yumi
---
examples/navigation/mobile_base_navigation.py | 8 +-
examples/ros/dummy_cmds_pub.py | 19 +++-
examples/ros/navigation_example.py | 99 +++++++++++++++----
.../smc/robots/implementations/mobile_yumi.py | 26 +++--
4 files changed, 118 insertions(+), 34 deletions(-)
diff --git a/examples/navigation/mobile_base_navigation.py b/examples/navigation/mobile_base_navigation.py
index 446cd99..05897d4 100644
--- a/examples/navigation/mobile_base_navigation.py
+++ b/examples/navigation/mobile_base_navigation.py
@@ -162,7 +162,13 @@ if __name__ == "__main__":
path_planner = partial(fixedPathPlanner, [0], path2D)
# CrocoBasePathFollowingMPC(args, robot, x0, path_planner)
- cartesianPathFollowingWithPlanner(args, robot, path_planner, 0.0)
+ loop = cartesianPathFollowingWithPlanner(args, robot, path_planner, 0.0, run=False)
+ breakFlag = False
+ import time
+
+ while not breakFlag:
+ loop.run_one_iter(loop.current_iteration)
+ time.sleep(robot._dt)
# MPCWithCLIKFallback(args, robot, path_planner)
if args.real:
diff --git a/examples/ros/dummy_cmds_pub.py b/examples/ros/dummy_cmds_pub.py
index 8b68dbc..2a42f46 100755
--- a/examples/ros/dummy_cmds_pub.py
+++ b/examples/ros/dummy_cmds_pub.py
@@ -65,6 +65,12 @@ def get_args():
default=False,
help="publish or not",
)
+ parser.add_argument(
+ "--sim",
+ type=float,
+ help="if in sim you need to set sim time, otherwise not",
+ default=3.0,
+ )
# parser.add_argument('--ros-args', action='extend', nargs="+")
# parser.add_argument('-r', action='extend', nargs="+")
parser.add_argument("--ros-args", action="append", nargs="*")
@@ -146,7 +152,6 @@ class DummyNode(Node):
JointState, f"{self._ns}/platform/joints_cmd", 1
)
self.robot.set_publisher_joints_cmd(self._cmd_pub)
- # self._cmd_pub = self.create_publisher(JointState, f"{self._ns}joints_cmd", 1)
self.sub_base_odom = self.create_subscription(
Odometry, f"{self._ns}/platform/odometry", self.callback_base_odom, 1
)
@@ -199,11 +204,15 @@ class DummyNode(Node):
]
)
th = 2 * np.arctan2(sinth2, costh2)
+ if np.linalg.norm(self.init_odom) < 1e-4:
+ self.init_odom[0] = msg.pose.pose.position.x
+ self.init_odom[1] = msg.pose.pose.position.y
+ self.init_odom[2] = th
if self.args.unreal:
- self.robot._q[0] = msg.pose.pose.position.x
- self.robot._q[1] = msg.pose.pose.position.y
- self.robot._q[2] = np.cos(th)
- self.robot._q[3] = np.sin(th)
+ self.robot._q[0] = msg.pose.pose.position.x - self.init_odom[0]
+ self.robot._q[1] = msg.pose.pose.position.y - self.init_odom[1]
+ self.robot._q[2] = np.cos(th - self.init_odom[2])
+ self.robot._q[3] = np.sin(th - self.init_odom[2])
# self.get_logger().info("ODOM CALLBACK" + str(msg))
def callback_arms_state(self, msg: JointState):
diff --git a/examples/ros/navigation_example.py b/examples/ros/navigation_example.py
index f20b10a..f1c225a 100644
--- a/examples/ros/navigation_example.py
+++ b/examples/ros/navigation_example.py
@@ -21,6 +21,7 @@ from sensor_msgs.msg import JointState
from nav_msgs.msg import Odometry
from abb_python_utilities.names import get_rosified_name
from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.time import Time
import numpy as np
import argparse
@@ -44,6 +45,10 @@ from smc.control.optimal_control.croco_path_following.mpc.base_reference_mpc imp
from smc.util.draw_path import drawPath
from smc.control.optimal_control.util import get_OCP_args
from smc.robots.utils import getRobotFromArgs
+from smc.control.cartesian_space.cartesian_space_trajectory_following import (
+ cartesianPathFollowingWithPlanner,
+)
+
import pinocchio as pin
import matplotlib
from functools import partial
@@ -114,6 +119,12 @@ def get_args():
help="height of the map in meters (y-axis) - only used for drawing of the path",
default=3.0,
)
+ parser.add_argument(
+ "--sim",
+ action=argparse.BooleanOptionalAction,
+ help="if in sim you need to set sim time, otherwise not",
+ default=True,
+ )
# parser.add_argument('--ros-args', action='extend', nargs="+")
# parser.add_argument('-r', action='extend', nargs="+")
parser.add_argument("--ros-args", action="append", nargs="*")
@@ -137,6 +148,15 @@ class DummyNode(Node):
self, args, robot: RealMobileYumiRobotManager, loop_manager: ControlLoopManager
):
super().__init__("dummy_cmds_pub_node")
+ if args.sim:
+ self.set_parameters(
+ [
+ rclpy.parameter.Parameter(
+ "use_sim_time", rclpy.Parameter.Type.BOOL, True
+ )
+ ]
+ )
+ self.wait_for_sim_time()
self.robot = robot
self.loop_manager = loop_manager
self.args = args
@@ -191,8 +211,7 @@ class DummyNode(Node):
JointState, f"{self._ns}/platform/joints_cmd", 1
)
self.get_logger().info(f"{self._ns}/platform/joints_cmd")
- self.robot.set_publisher_joints_cmd(self._cmd_pub)
- self._cmd_pub = self.create_publisher(JointState, f"{self._ns}joints_cmd", 1)
+ # self.robot.set_publisher_joints_cmd(self._cmd_pub)
self.sub_base_odom = self.create_subscription(
Odometry, f"{self._ns}/platform/odometry", self.callback_base_odom, 1
)
@@ -214,14 +233,25 @@ class DummyNode(Node):
breakFlag = self.loop_manager.run_one_iter(
self.loop_manager.current_iteration
)
+ # self.get_logger().info("TIMER_CMD")
- # msg = self.empty_msg
- # for i in range(3):
- # msg.velocity[i] = self.robot._v[i]
- # for i in range(15, 29):
- # msg.velocity[i] = self.robot._v[i - 12]
- # self.get_logger().info(str(self.robot._v))
- # self._cmd_pub.publish(msg)
+ if self.args.unreal:
+ msg = self.empty_msg
+ msg.header.stamp = Time().to_msg()
+
+ # TEST
+ # msg.velocity[2] = (
+ # np.sin(self.loop_manager.current_iteration / (self.args.ctrl_freq * 3))
+ # / 10
+ # )
+ # REAL
+ for i in range(3):
+ msg.velocity[i] = self.robot._v_cmd[i]
+ for i in range(15, 29):
+ msg.velocity[i] = self.robot._v_cmd[i - 12]
+ self.get_logger().info(str(msg))
+ self.get_logger().info(str(self.robot._v_cmd))
+ self._cmd_pub.publish(msg)
def callback_base_odom(self, msg: Odometry):
# self.robot._v[0] = msg.twist.twist.linear.x
@@ -239,22 +269,47 @@ class DummyNode(Node):
]
)
th = 2 * np.arctan2(sinth2, costh2)
- if np.linalg.norm(self.init_odom) < 1e-4:
+ if (
+ np.linalg.norm(self.init_odom) < 1e-4
+ ) and self.loop_manager.current_iteration < 10:
self.init_odom[0] = msg.pose.pose.position.x
self.init_odom[1] = msg.pose.pose.position.y
self.init_odom[2] = th
if self.args.unreal:
- self.robot._q[0] = msg.pose.pose.position.x - self.init_odom[0]
- self.robot._q[1] = msg.pose.pose.position.y - self.init_odom[1]
+ T_odom = np.zeros((3, 3))
+ T_odom[0, 0] = np.cos(self.init_odom[2])
+ T_odom[0, 1] = -1 * np.sin(self.init_odom[2])
+ T_odom[0, 2] = self.init_odom[0]
+ T_odom[1, 0] = np.sin(self.init_odom[2])
+ T_odom[1, 1] = np.cos(self.init_odom[2])
+ T_odom[1, 2] = self.init_odom[1]
+ T_odom[2, 2] = 1.0
+ p_odom = np.array([msg.pose.pose.position.x, msg.pose.pose.position.y, 1])
+ T_inv_odom = np.zeros((3, 3))
+ T_inv_odom[:2, :2] = T_odom[:2, :2].T
+ T_inv_odom[:2, 2] = (-1 * T_odom[:2, :2].T) @ T_odom[:2, 2]
+ T_inv_odom[2, 2] = 0.0
+ p_ctrl = T_inv_odom @ p_odom
+ self.robot._q[0] = p_ctrl[0]
+ self.robot._q[1] = p_ctrl[1]
self.robot._q[2] = np.cos(th - self.init_odom[2])
self.robot._q[3] = np.sin(th - self.init_odom[2])
+ # self.get_logger().info("CALLBACK_ODOM")
+ self.get_logger().info(str(self.robot._q[:4]))
+ # self.get_logger().info(str(self.init_odom))
def callback_arms_state(self, msg: JointState):
- if self.args.unreal:
- if self.loop_manager.current_iteration < 200 or self.args.unreal:
- self.robot._q[-14:-7] = msg.position[-14:-7]
- self.robot._q[-7:] = msg.position[-7:]
- # self.get_logger().info(str(self.loop_manager.current_iteration))
+ if self.loop_manager.current_iteration < 200 or self.args.unreal:
+ self.robot._q[-14:-7] = msg.position[-14:-7]
+ self.robot._q[-7:] = msg.position[-7:]
+ # self.get_logger().info("CALLBACK_JOINT_STATE")
+
+ def wait_for_sim_time(self):
+ """Wait for the /clock topic to start publishing."""
+ self.get_logger().info("Waiting for simulated time to be active...")
+ while not self.get_clock().now().nanoseconds > 0:
+ rclpy.spin_once(self, timeout_sec=0.1)
+ self.get_logger().info("Simulated time is now active!")
# def main(args=None):
@@ -294,9 +349,13 @@ def main(args=None):
robot.updateViz({"fixed_path": path2D})
path_planner = partial(fixedPathPlanner, [0], path2D)
- x0 = np.concatenate([robot.q, robot.v])
- loop_manager = CrocoBasePathFollowingMPC(
- args_smc, robot, x0, path_planner, run=False
+ # x0 = np.concatenate([robot.q, robot.v])
+ # loop_manager = CrocoBasePathFollowingMPC(
+ # args_smc, robot, x0, path_planner, run=False
+ # )
+ robot._mode = robot.control_mode.base_only
+ loop_manager = cartesianPathFollowingWithPlanner(
+ args_smc, robot, path_planner, 0.0, run=False
)
rclpy.init(args=args)
diff --git a/python/smc/robots/implementations/mobile_yumi.py b/python/smc/robots/implementations/mobile_yumi.py
index 35671a7..602fdee 100644
--- a/python/smc/robots/implementations/mobile_yumi.py
+++ b/python/smc/robots/implementations/mobile_yumi.py
@@ -103,9 +103,15 @@ class SimulatedMobileYuMiRobotManager(
)
# pin.RandomConfiguration does not work well for planar joint,
# or at least i remember something along those lines being the case
- self._q[0] = np.random.random()
- self._q[1] = np.random.random()
- theta = np.random.random() * 2 * np.pi - np.pi
+ # self._q[0] = np.random.random()
+ # self._q[1] = np.random.random()
+ # theta = np.random.random() * 2 * np.pi - np.pi
+ # self._q[2] = np.cos(theta)
+ # self._q[3] = np.sin(theta)
+
+ self._q[0] = np.random.random() * 0.0
+ self._q[1] = np.random.random() * 0.0
+ theta = np.random.random() * 0.0
self._q[2] = np.cos(theta)
self._q[3] = np.sin(theta)
@@ -113,6 +119,7 @@ class SimulatedMobileYuMiRobotManager(
class RealMobileYumiRobotManager(AbstractMobileYuMiRobotManager):
def __init__(self, args):
super().__init__(args)
+ self._v_cmd = np.zeros(self.model.nv)
# TODO: here assert you need to have ros2 installed to run on real heron
# and then set all this bullshit here instead of elsewhere
@@ -125,7 +132,7 @@ class RealMobileYumiRobotManager(AbstractMobileYuMiRobotManager):
if self.args.unreal:
if any(np.isnan(v)):
v = np.zeros(self.model.nv)
- self._v = v
+ self._v_cmd = v
# empty_msg = JointState()
# for i in range(29):
# empty_msg.velocity.append(0.0)
@@ -152,7 +159,7 @@ class RealMobileYumiRobotManager(AbstractMobileYuMiRobotManager):
pass
def stopRobot(self):
- self.sendVelocityCommand(np.zeros(self.model.nv))
+ self.sendVelocityCommand(np.zeros(self.nv))
def setFreedrive(self):
pass
@@ -169,9 +176,12 @@ class RealMobileYumiRobotManager(AbstractMobileYuMiRobotManager):
)
# pin.RandomConfiguration does not work well for planar joint,
# or at least i remember something along those lines being the case
- self._q[0] = np.random.random() * 0.1
- self._q[1] = np.random.random() * 0.1
- theta = np.random.random() * 2 * np.pi - np.pi
+ # self._q[0] = np.random.random() * 0.1
+ # self._q[1] = np.random.random() * 0.1
+ # theta = np.random.random() * 2 * np.pi - np.pi
+ self._q[0] = np.random.random() * 0.0
+ self._q[1] = np.random.random() * 0.0
+ theta = np.random.random() * 0.0
self._q[2] = np.cos(theta)
self._q[3] = np.sin(theta)
--
GitLab