From 42ac3667726884f5f627136e5ab850f6c9a8679b Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Mon, 10 Mar 2025 08:35:20 +0100
Subject: [PATCH] realyumirobotmanager + dummy publisher and subscriber
---
examples/ros/dummy_cmds_pub.py | 83 +++++++++----------
python/smc.egg-info/PKG-INFO | 7 +-
python/smc.egg-info/SOURCES.txt | 29 +------
.../smc/robots/implementations/mobile_yumi.py | 32 ++++++-
python/smc/robots/utils.py | 4 +-
5 files changed, 77 insertions(+), 78 deletions(-)
diff --git a/examples/ros/dummy_cmds_pub.py b/examples/ros/dummy_cmds_pub.py
index d9c34f8..0c4653e 100755
--- a/examples/ros/dummy_cmds_pub.py
+++ b/examples/ros/dummy_cmds_pub.py
@@ -20,12 +20,17 @@ from geometry_msgs.msg import Twist, PoseWithCovarianceStamped
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
import numpy as np
import argparse
-from smc.managers import getMinimalArgParser, ControlLoopManager, RobotManager
-from smc.clik.clik import (
- getClikArgs,
+from smc.robots.interfaces.whole_body_dual_arm_interface import (
+ DualArmWholeBodyInterface,
+)
+from smc import getMinimalArgParser
+from smc.control.control_loop_manager import ControlLoopManager
+from smc.control.cartesian_space import getClikArgs
+from smc.control.cartesian_space.cartesian_space_point_to_point import (
getClikController,
controlLoopClik,
moveL,
@@ -34,20 +39,12 @@ from smc.clik.clik import (
invKinmQP,
moveLDualArm,
)
-from smc.optimal_control.get_ocp_args import get_OCP_args
-from smc.optimal_control.crocoddyl_mpc import CrocoIKMPC
+from smc.control.optimal_control.util import get_OCP_args
+from smc.robots.utils import getRobotFromArgs
import pinocchio as pin
import numpy as np
-import rclpy
-from sensor_msgs.msg import JointState
-from rclpy.callback_groups import ReentrantCallbackGroup
-from rclpy.executors import MultiThreadedExecutor
-from rclpy.node import Node
-
-from abb_python_utilities.names import get_rosified_name
-
# you will need to manually set everything here :(
def get_args():
@@ -68,15 +65,16 @@ def get_args():
parser.add_argument("-r", action="append", nargs="*")
parser.add_argument("-p", action="append", nargs="*")
args = parser.parse_args()
- args.robot = "yumi"
+ args.robot = "myumi"
args.save_log = True
- args.real_time_plotting = False
+ args.plotter = True
+ args.real = True
return args
class DummyNode(Node):
def __init__(
- self, args, robot_manager: RobotManager, loop_manager: ControlLoopManager
+ self, args, robot: DualArmWholeBodyInterface, loop_manager: ControlLoopManager
):
super().__init__("dummy_cmds_pub_node")
@@ -103,8 +101,8 @@ class DummyNode(Node):
# connect to smc
###########################################################
- robot_manager.set_publisher_joints_cmd(self._cmd_pub)
- self.robot_manager = robot_manager
+ robot.set_publisher_joints_cmd(self._cmd_pub)
+ self.robot = robot
self.loop_manager = loop_manager
self.args = args
# give me the latest thing even if there wasn't an update
@@ -150,45 +148,44 @@ class DummyNode(Node):
# self._t += self._dt
def callback_base_odom(self, msg: Odometry):
- self.robot_manager.v_q[0] = msg.twist.twist.linear.x
- self.robot_manager.v_q[1] = msg.twist.twist.linear.y
+ self.robot._v[0] = msg.twist.twist.linear.x
+ self.robot._v[1] = msg.twist.twist.linear.y
# TODO: check that z can be used as cos(theta) and w as sin(theta)
# (they could be defined some other way or theta could be offset of something)
- self.robot_manager.v_q[2] = msg.twist.twist.angular.z
- self.robot_manager.v_q[3] = 0 # for consistency
+ self.robot._v[2] = msg.twist.twist.angular.z
+ self.robot._v[3] = 0 # for consistency
print("received /odom")
def callback_arms_state(self, msg: JointState):
- self.robot_manager.q[0] = 0.0
- self.robot_manager.q[1] = 0.0
- self.robot_manager.q[2] = 1.0
- self.robot_manager.q[3] = 0.0
- self.robot_manager.q[-14:-7] = msg.position[-14:-7]
- self.robot_manager.q[-7:] = msg.position[-7:]
- self.get_logger().info("jebem ti mamu")
+ self.robot._q[0] = 0.0
+ self.robot._q[1] = 0.0
+ self.robot._q[2] = 1.0
+ self.robot._q[3] = 0.0
+ self.robot._q[-14:-7] = msg.position[-14:-7]
+ self.robot._q[-7:] = msg.position[-7:]
+ self.get_logger().info("callback arms state")
# def main(args=None):
def main(args=None):
# evil and makes args unusable but what can you do
args_smc = get_args()
- # print(args_smc)
- rclpy.init(args=args)
-
- robot = RobotManager(args_smc)
+ assert args_smc.robot == "myumi"
+ robot = getRobotFromArgs(args_smc)
# you can't do terminal input with ros
# goal = robot.defineGoalPointCLI()
- goal = pin.SE3.Identity()
- goal.translation = np.array([0.3, 0.0, 0.5])
- if robot.robot_name == "yumi":
- goal.rotation = np.eye(3)
- goal_transform = pin.SE3.Identity()
- goal_transform.translation[1] = 0.1
- goal_left = goal_transform.act(goal)
- goal_left = goal_transform.inverse().act(goal)
- # goal = (goal_left, goal_right)
+ T_w_absgoal = pin.SE3.Identity()
+ T_w_absgoal.translation = np.array([0.3, 0.0, 0.5])
+ T_absgoal_l = pin.SE3.Identity()
+ T_absgoal_l.translation = np.array([-0.15, 0.0, 0.0])
+ T_absgoal_r = pin.SE3.Identity()
+ T_absgoal_r.translation = np.array([0.15, 0.0, 0.0])
# loop_manager = CrocoIKMPC(args, robot, goal, run=False)
- loop_manager = moveLDualArm(args_smc, robot, goal, goal_transform, run=False)
+ loop_manager = moveLDualArm(
+ args_smc, robot, T_w_absgoal, T_absgoal_l, T_absgoal_r, run=False
+ )
+
+ rclpy.init(args=args)
executor = MultiThreadedExecutor()
node = DummyNode(args, robot, loop_manager)
diff --git a/python/smc.egg-info/PKG-INFO b/python/smc.egg-info/PKG-INFO
index d5f66fa..c91e812 100644
--- a/python/smc.egg-info/PKG-INFO
+++ b/python/smc.egg-info/PKG-INFO
@@ -1,7 +1,12 @@
Metadata-Version: 2.1
Name: smc
-Version: 0.2
+Version: 0.3
Summary: Simple Manipulator Control (SMC) - simplest possible framework for robot control.
Home-page: https://gitlab.control.lth.se/marko-g/ur_simple_control
Author: Marko Guberina
+License: UNKNOWN
+Platform: UNKNOWN
License-File: LICENSE.txt
+
+UNKNOWN
+
diff --git a/python/smc.egg-info/SOURCES.txt b/python/smc.egg-info/SOURCES.txt
index 7063d2e..7a0fec2 100644
--- a/python/smc.egg-info/SOURCES.txt
+++ b/python/smc.egg-info/SOURCES.txt
@@ -76,11 +76,7 @@ smc/control/joint_space/joint_space_trajectory_following.py
smc/control/optimal_control/__init__.py
smc/control/optimal_control/abstract_croco_ocp.py
smc/control/optimal_control/create_pinocchio_casadi_ocp.py
-smc/control/optimal_control/croco_mpc_path_following.py
-smc/control/optimal_control/croco_mpc_point_to_point.py
smc/control/optimal_control/notes.md
-smc/control/optimal_control/path_following_croco_ocp.py
-smc/control/optimal_control/point_to_point_croco_ocp.py
smc/control/optimal_control/util.py
smc/logging/__init__.py
smc/logging/logger.py
@@ -258,9 +254,7 @@ smc/path_generation/starworlds/utils/__init__.py
smc/path_generation/starworlds/utils/cg.py
smc/path_generation/starworlds/utils/misc.py
smc/robots/__init__.py
-smc/robots/abstract_simulated_robot.py
smc/robots/notes_on_getting_model_from_urdf.md
-smc/robots/robotmanager_abstract.py
smc/robots/utils.py
smc/robots/grippers/__init__.py
smc/robots/grippers/abstract_gripper.py
@@ -326,31 +320,10 @@ smc/vision/__init__.py
smc/vision/vision.py
smc/visualization/__init__.py
smc/visualization/manipulator_comparison_visualizer.py
-smc/visualization/manipulator_visual_motion_analyzer.py
-smc/visualization/path_visualizer.py
smc/visualization/plotters.py
smc/visualization/visualizer.py
-smc/visualization/arms/another_debug_dh
-smc/visualization/arms/j2n6s300_dh_params
-smc/visualization/arms/j2s7s300_dh_params
-smc/visualization/arms/kinova_jaco_params
-smc/visualization/arms/kuka_lbw_iiwa_dh_params
-smc/visualization/arms/lbr_iiwa_jos_jedan_dh
-smc/visualization/arms/robot_parameters2
-smc/visualization/arms/testing_dh_parameters
-smc/visualization/arms/ur10e_dh_parameters_from_the_ur_site
-smc/visualization/arms/ur5e_dh
smc/visualization/meshcat_viewer_wrapper/__init__.py
smc/visualization/meshcat_viewer_wrapper/colors.py
smc/visualization/meshcat_viewer_wrapper/visualizer.py
smc/visualization/obsolete_to_be_removed/main.py
-smc/visualization/obsolete_to_be_removed/make_run.py
-smc/visualization/robot_stuff/InverseKinematics.py
-smc/visualization/robot_stuff/drawing.py
-smc/visualization/robot_stuff/drawing_for_anim.py
-smc/visualization/robot_stuff/follow_curve.py
-smc/visualization/robot_stuff/forw_kinm.py
-smc/visualization/robot_stuff/inv_kinm.py
-smc/visualization/robot_stuff/joint_as_hom_mat.py
-smc/visualization/robot_stuff/utils.py
-smc/visualization/robot_stuff/webots_api_helper_funs.py
\ No newline at end of file
+smc/visualization/obsolete_to_be_removed/make_run.py
\ No newline at end of file
diff --git a/python/smc/robots/implementations/mobile_yumi.py b/python/smc/robots/implementations/mobile_yumi.py
index 8c62ce2..f1d6850 100644
--- a/python/smc/robots/implementations/mobile_yumi.py
+++ b/python/smc/robots/implementations/mobile_yumi.py
@@ -12,6 +12,10 @@ import pinocchio as pin
from argparse import Namespace
import numpy as np
+if find_spec("rclpy"):
+ from rclpy.time import Time
+ from sensor_msgs.msg import JointState
+
class AbstractMobileYuMiRobotManager(DualArmWholeBodyInterface):
@@ -115,7 +119,7 @@ class RealMobileYumiRobotManager(AbstractMobileYuMiRobotManager):
self.publisher_joints_cmd = publisher_joints_cmd
print("set publisher_joints_cmd into RobotManager")
- def sendVelocityCommand(self, v):
+ def sendVelocityCommandToReal(self, v: np.ndarray):
# qd_base = qd[:3]
# qd_left = qd[3:10]
# qd_right = qd[10:]
@@ -128,13 +132,35 @@ class RealMobileYumiRobotManager(AbstractMobileYuMiRobotManager):
msg = empty_msg
msg.header.stamp = Time().to_msg()
for i in range(3):
- msg.velocity[i] = qd_cmd[i]
+ msg.velocity[i] = v[i]
for i in range(15, 29):
- msg.velocity[i] = qd_cmd[i - 12]
+ msg.velocity[i] = v[i - 12]
self.publisher_joints_cmd.publish(msg)
# TODO: define set initial pose by reading it from the real robot (well, the appropriate ros2 topic in this case)
+ def _updateQ(self):
+ pass
+
+ def _updateV(self):
+ pass
+
+ def stopRobot(self):
+ self.sendVelocityCommand(np.zeros(self.model.nv))
+
+ def setFreedrive(self):
+ pass
+
+ def unSetFreedrive(self):
+ pass
+
+ def connectToRobot(self):
+ pass
+
+ # TODO: create simulated gripper class to support the move, open, close methods - it's a mess now
+ # in simulation we can just set the values directly
+ def connectToGripper(self):
+ pass
# TODO: define a separate mobile base for YuMi here
diff --git a/python/smc/robots/utils.py b/python/smc/robots/utils.py
index c0df4b4..4d53987 100644
--- a/python/smc/robots/utils.py
+++ b/python/smc/robots/utils.py
@@ -278,9 +278,7 @@ def getRobotFromArgs(args: argparse.Namespace) -> AbstractRobotManager:
return SimulatedYuMiRobotManager(args)
if args.robot == "myumi":
if args.real:
- pass
- # TODO: finish it
- # return RealMobileYuMiRobotManager(args)
+ return RealMobileYuMiRobotManager(args)
else:
return SimulatedMobileYuMiRobotManager(args)
if args.robot == "mir":
--
GitLab