Merge branch 'pp' of https://gitlab.control.lth.se/marko-g/ur_simple_control into pp

merge is necessary because fast-forward was not possible

python/examples/dummy_cmds_pub.py

+#!/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()

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)