From c69b522037db263b93fd887f515988a485e0550b Mon Sep 17 00:00:00 2001
From: Your Name <marko.guberina@control.lth.se>
Date: Wed, 11 Dec 2024 18:08:17 +0100
Subject: [PATCH] integration day 2 at abb end
---
python/examples/dummy_cmds_pub.py | 76 +++++++++++++++++++++++++++
python/examples/smc_yumi_challenge.py | 16 ++++--
2 files changed, 88 insertions(+), 4 deletions(-)
create mode 100755 python/examples/dummy_cmds_pub.py
diff --git a/python/examples/dummy_cmds_pub.py b/python/examples/dummy_cmds_pub.py
new file mode 100755
index 0000000..a3aa151
--- /dev/null
+++ b/python/examples/dummy_cmds_pub.py
@@ -0,0 +1,76 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) 2024, ABB Schweiz AG
+# All rights reserved.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
+# IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
+# FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+# CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+# WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
+# WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+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
+
+
+class DummyNode(Node):
+ def __init__(self):
+ super().__init__("dummy_cmds_pub_node")
+
+ self._cb = ReentrantCallbackGroup()
+
+ self._ns = get_rosified_name(self.get_namespace())
+ # test
+ self._ns = "maskinn"
+
+ self._cmd_pub = self.create_publisher(JointState, f"{self._ns}/joints_cmd", 1)
+
+ self.get_logger().info(f"### Starting dummy example under namespace {self._ns}")
+
+ self.empty_msg = JointState()
+ for i in range(29):
+ self.empty_msg.velocity.append(0.0)
+
+ self._T = 1.0
+ self._A = 0.2
+ self._t = 0.0
+ self._dt = 0.004
+
+ self._pub_timer = self.create_timer(self._dt, self.send_cmd)
+
+ def send_cmd(self):
+ msg = self.empty_msg
+ msg.header.stamp = self.get_clock().now().to_msg()
+ msg.velocity[0] = 1.0
+ msg.velocity[1] = 1.0
+ msg.velocity[2] = 1.0
+ msg.velocity[3] = 1.0
+ msg.velocity[4] = 1.0
+ msg.velocity[5] = 1.0
+ msg.velocity[21] = self._A * np.sin(2 * np.pi / self._T * self._t)
+ msg.velocity[28] = self._A * np.sin(2 * np.pi / self._T * self._t)
+ self._cmd_pub.publish(msg)
+ #print("published")
+ self._t += self._dt
+
+
+def main(args=None):
+ rclpy.init(args=args)
+ executor = MultiThreadedExecutor()
+ node = DummyNode()
+ executor.add_node(node)
+ executor.spin()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/python/examples/smc_yumi_challenge.py b/python/examples/smc_yumi_challenge.py
index 06ae8f7..2af6a7c 100644
--- a/python/examples/smc_yumi_challenge.py
+++ b/python/examples/smc_yumi_challenge.py
@@ -16,6 +16,13 @@ from ur_simple_control.optimal_control.crocoddyl_mpc import CrocoIKMPC
import pinocchio as pin
+###########################################################
+# this is how to get the positions ros2 topic echo /maskinn/auxiliary/joint_states_merged
+# this is how to send commands: /maskinn/auxiliary/robot_description
+#self._cmd_pub = self.create_publisher(JointState, f"{self._ns}/joints_cmd", 1)
+#############################################33
+
+
def get_args():
parser = getMinimalArgParser()
parser.description = 'Run closed loop inverse kinematics \
@@ -30,7 +37,7 @@ def get_args():
class ROSCommHandlerMYumi(Node):
def __init__(self, args, robot_manager : RobotManager, loop_manager : ControlLoopManager):
- super().__init__('ROSCommHandlerHeron')
+ super().__init__('ROSCommHandlerMYumi')
# does not work
#self._ns = get_rosified_name(self.get_namespace())
@@ -39,9 +46,10 @@ class ROSCommHandlerMYumi(Node):
self.get_logger().info(f"### Starting dummy example under namespace {self._ns}")
#self.pub_vel_base = self.create_publisher(Twist, f"{self._ns}platform/base_command", 1)
- self.pub_vel_base = self.create_publisher(Twist, f"{self._ns}platform/command_limiter_node/base/cmd_vel_in_navigation", 1)
- self.pub_vel_left = self.create_publisher(JointState, f"{self._ns}platform/left_arm_command", 1)
- self.pub_vel_right = self.create_publisher(JointState, f"{self._ns}platform/right_arm_command", 1)
+ #self.pub_vel_base = self.create_publisher(Twist, f"{self._ns}platform/command_limiter_node/base/cmd_vel_in_navigation", 1)
+ #self.pub_vel_left = self.create_publisher(JointState, f"{self._ns}platform/left_arm_command", 1)
+ #self.pub_vel_right = self.create_publisher(JointState, f"{self._ns}platform/right_arm_command", 1)
+
robot_manager.set_publisher_vel_base(self.pub_vel_base)
robot_manager.set_publisher_vel_left(self.pub_vel_left)
robot_manager.set_publisher_vel_right(self.pub_vel_right)
--
GitLab