Noetic devel

mur/mur_control/launch/global_tcp_pose_publisher.launch

@@ -0,0 +1,22 @@
+<launch>
+    <arg name="world_T_base_topic" default="/mur620/mir_pose_stamped_simple"/>
+    <arg name="base_T_ee_topic" default="/mur620/UR10_l/tcp_pose"/> <!-- TODO: could be done with static transform in future -->
+    <arg name="world_T_ee_topic" default="/global_nozzle_pose"/>
+    <arg name="static_tf_included" default="true"/>
+    <arg name="static_tf_parent" default="mur620/base_footprint"/>
+    <arg name="static_tf_child" default="mur620/UR10_l/base_link"/>
+    <arg name="world_frame" default="map"/> <!-- just for publisher frame -->
+
+    <node name="pose_composer" pkg="mur_control" type="pose_composer.py" output="screen">
+        <!-- Topic Remappings -->
+        <remap from="world_T_base" to="$(arg world_T_base_topic)"/>
+        <remap from="base_T_ee" to="$(arg base_T_ee_topic)"/>
+        <remap from="world_T_ee" to="$(arg world_T_ee_topic)"/><!-- publisher -->
+
+        <param name="world_frame" value="$(arg world_frame)"/>
+
+        <param name="static_tf_included" value="$(arg static_tf_included)"/>
+        <param name="static_tf_parent" value="$(arg static_tf_parent)"/>
+        <param name="static_tf_child" value="$(arg static_tf_child)"/>
+    </node>
+</launch>

mur/mur_control/launch/multi_twist_controller_sim.launch

@@ -0,0 +1,36 @@
+<launch>
+
+
+    <arg name="robot1_name"  default="mur620a"/>
+    <arg name="robot2_name"  default="mur620b"/>
+
+
+    <include file="$(find mur_control)/launch/twist_controller_sim.launch">
+        <arg name="robot_name"  value="$(arg robot1_name)"/>
+        <arg name="group_vel_controller_topic" value="/$(arg robot1_name)/joint_group_vel_controller_l/unsafe/command"/>
+        <arg name="commanded_twist_topic" value="/$(arg robot1_name)/UR10_l/commanded_twist"/>
+        <arg name="move_group_name" value="UR_arm_l"/>
+    </include>
+
+    <include file="$(find mur_control)/launch/twist_controller_sim.launch">
+        <arg name="robot_name"  value="$(arg robot1_name)"/>
+        <arg name="group_vel_controller_topic" value="/$(arg robot1_name)/joint_group_vel_controller_r/unsafe/command"/>
+        <arg name="commanded_twist_topic" value="/$(arg robot1_name)/UR10_r/commanded_twist"/>
+        <arg name="move_group_name" value="UR_arm_r"/>
+    </include>
+
+    <include file="$(find mur_control)/launch/twist_controller_sim.launch">
+        <arg name="robot_name"  value="$(arg robot2_name)"/>
+        <arg name="group_vel_controller_topic" value="/$(arg robot2_name)/joint_group_vel_controller_l/unsafe/command"/>
+        <arg name="commanded_twist_topic" value="/$(arg robot2_name)/UR10_l/commanded_twist"/>
+        <arg name="move_group_name" value="UR_arm_l"/>
+    </include>
+
+    <include file="$(find mur_control)/launch/twist_controller_sim.launch">
+        <arg name="robot_name"  value="$(arg robot2_name)"/>
+        <arg name="group_vel_controller_topic" value="/$(arg robot2_name)/joint_group_vel_controller_r/unsafe/command"/>
+        <arg name="commanded_twist_topic" value="/$(arg robot2_name)/UR10_r/commanded_twist"/>
+        <arg name="move_group_name" value="UR_arm_r"/>
+    </include>
+
+</launch>

mur/mur_control/launch/twist_controller_sim.launch

@@ -0,0 +1,18 @@
+<launch>
+
+    <arg name="robot_name"  default="mur620a"/>
+    <arg name="group_vel_controller_topic" default="/$(arg robot_name)/joint_group_vel_controller_r/unsafe/command"/>
+    <arg name="commanded_twist_topic" default="/$(arg robot_name)/UR10_r/commanded_twist"/>
+    <arg name="move_group_name" default="UR_arm_r"/>
+
+    <group ns="$(arg robot_name)">
+
+        <node name="twist_controller_sim_$(arg robot_name)_$(arg move_group_name)" pkg="mur_control" type="twist_controller_sim.py" output="screen">
+            <param name="robot_name" value="$(arg robot_name)" />
+            <param name="group_vel_controller_topic" value="$(arg group_vel_controller_topic)" />
+            <param name="commanded_twist_topic" value="$(arg commanded_twist_topic)" />
+            <param name="move_group_name" value="$(arg move_group_name)" />  
+        </node>
+    </group>
+
+</launch>

mur/mur_control/scripts/global_tcp_pose_publisher.py

@@ -13,7 +13,7 @@ def __init__(self):
         self.UR_base_link_name = rospy.get_param('~UR_base_link_name', 'mur620a/UR10_l/base_link_inertia')
         self.base_frame = rospy.get_param('~base_frame', 'map')
         self.local_TCP_pose_topic = rospy.get_param('~local_TCP_pose_topic', '/mur620a/UR10_l/ur_calibrated_pose')
-        self.static_mode = rospy.get_param('~static_mode', False)
+        self.static_mode = rospy.get_param('~static_mode', True)
 
         self.local_TCP_pose = Pose()
 

mur/mur_control/scripts/pose_composer.py

@@ -0,0 +1,84 @@
+#!/usr/bin/env python3
+import rospy
+
+from geometry_msgs.msg import Pose, PoseStamped
+from tf import TransformListener
+from tf.transformations import quaternion_matrix, quaternion_from_matrix, concatenate_matrices
+
+import numpy as np
+
+# Listens to Pose messages and calculates the kinematic chain to publish the total Pose
+# in: world_T_base, base_T_ee; out: world_T_ee
+def pose_to_matrix(pose:Pose):
+    """Convert Pose to transformation matrix"""
+    trans = [pose.position.x, pose.position.y, pose.position.z]
+    rot = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
+    matrix = quaternion_matrix(rot)
+    matrix[0:3, 3] = trans
+    return matrix
+
+def matrix_to_pose(matrix: np.ndarray) -> Pose:
+    """Convert transformation matrix to Pose"""
+    pose = Pose()
+    translation = matrix[0:3, 3]
+    rotation = quaternion_from_matrix(matrix)
+    pose.position.x, pose.position.y, pose.position.z = translation
+    pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w = rotation
+    return pose
+class TotalPose:
+    def __init__(self):
+        # msg_class_base,_,_=rostopic.get_topic_class("world_T_base", blocking=True)
+        self.world_T_base = np.eye(4)
+        self.base_T_ee = np.eye(4)
+        self.world_T_ee = PoseStamped()
+        self.world_T_ee.header.frame_id = rospy.get_param("~world_frame", "map")
+
+        # In case there is a static transform between the 2 topics:
+        self.static_tf_included = rospy.get_param("~static_tf_included", False)
+        static_tf_parent = rospy.get_param("~static_tf_parent", "mur620/base_footprint")
+        static_tf_child = rospy.get_param("~static_tf_child", "mur620/UR10_l/base_link")
+        self.static_T = None
+        if self.static_tf_included:
+            tf_listener = TransformListener()
+
+            while not rospy.is_shutdown():
+                try:
+                    tf_listener.waitForTransform(static_tf_parent, static_tf_child, rospy.Time(0), rospy.Duration(5.0))
+                    static_trans, static_rot = tf_listener.lookupTransform(static_tf_parent, static_tf_child, rospy.Time(0))
+                    break
+                except Exception as e:
+                    rospy.logwarn(f"Could not get static transform: {e}. Retrying...")
+                    rospy.sleep(1.0)
+            self.static_T = quaternion_matrix([static_rot[0], static_rot[1], static_rot[2], static_rot[3]])
+            self.static_T[0:3, 3] = static_trans
+
+        self.world_T_base_sub = rospy.Subscriber("world_T_base", PoseStamped, self.world_T_base_callback)
+        self.base_T_ee_sub = rospy.Subscriber("base_T_ee", PoseStamped, self.base_T_ee_callback)
+        self.world_T_ee_pub = rospy.Publisher("world_T_ee", PoseStamped, queue_size=1)
+
+    def world_T_base_callback(self, msg: PoseStamped):
+        self.world_T_base = pose_to_matrix(msg.pose)
+        self.world_T_ee.header.stamp = msg.header.stamp
+        self.update_world_T_ee()
+
+    def base_T_ee_callback(self, msg: PoseStamped):
+        base_T_ee = pose_to_matrix(msg.pose)
+        if self.static_tf_included:
+            base_T_ee = concatenate_matrices(self.static_T, base_T_ee)
+        self.base_T_ee = base_T_ee
+
+        self.world_T_ee.header.stamp = msg.header.stamp
+        self.update_world_T_ee()
+
+    def update_world_T_ee(self):
+
+        # self.world_T_ee.pose = transform_pose_by_pose(self.world_T_base, self.base_T_ee, (False, False))
+
+        world_T_ee = concatenate_matrices(self.world_T_base, self.base_T_ee)    
+        self.world_T_ee.pose = matrix_to_pose(world_T_ee)
+        self.world_T_ee_pub.publish(self.world_T_ee)
+
+if __name__ == "__main__":
+    rospy.init_node("total_pose")
+    total_pose = TotalPose()
+    rospy.spin()

mur/mur_description/urdf/mur_620.gazebo.xacro

@@ -34,6 +34,7 @@
 	<xacro:if value="${sim}">
 		<xacro:include filename="$(find schunk_emh_rp_45)/urdf/schunk_emh_rp_045.urdf.xacro"/>
 		<xacro:include filename="$(find keyence_lj_x)/urdf/keyence_lj_x.urdf.xacro"/>
+		<xacro:include filename="$(find bauschaum_ee)/urdf/bauschaum_ee.urdf.xacro"/>
 	</xacro:if>
 	<!-- Init the MiR200 marco that is also used for the hardware -->
 	<xacro:mir_600 />
@@ -106,6 +107,10 @@
 			<xacro:keyence_lj_x tf_prefix="UR10_r"	tool_link="UR10_r/flange"/>
 			<xacro:keyence_lj_x tf_prefix="UR10_l"	tool_link="UR10_l/flange"/>
 		</xacro:if>
+		<xacro:if value="${tool == 'bauschaum_ee'}">
+			<xacro:bauschaum_ee tf_prefix="UR10_r"	tool_link="UR10_r/flange"/>
+			<xacro:bauschaum_ee  tf_prefix="UR10_l"	tool_link="UR10_l/flange"/>
+		</xacro:if>
 	</xacro:if>
 
     <!-- <xacro:include filename="$(find print_sim)/urdf/keyence_line_laser.gazebo.xacro" /> 

mur/mur_examples/scripts/set_virtual_object_pose.py

@@ -5,22 +5,25 @@
 def set_target_pose():
     # Initialize the ROS node
     rospy.init_node('set_virtual_object_pose')
-
+    pose = rospy.get_param('~pose', [0.0, 0.0, 1.5, 0, 0, 0.3826834, 0.9238795])
+    pose_topic = rospy.get_param('~pose_topic', '/virtual_object/set_pose')
     # Create a publisher for the target pose
-    pub = rospy.Publisher('/virtual_object/set_pose', PoseStamped, queue_size=10)
+    pub = rospy.Publisher(pose_topic, PoseStamped, queue_size=10)
+
+    rospy.loginfo('Setting the target pose to: {}'.format(pose))
 
     rospy.sleep(1)
 
     # Create a PoseStamped message
     target_pose = PoseStamped()
     target_pose.header.frame_id = 'map'  # Set the frame ID
-    target_pose.pose.position.x = 0.0  # Set the position (x-coordinate)
-    target_pose.pose.position.y = 0.0  # Set the position (y-coordinate)
-    target_pose.pose.position.z = 1.5  # Set the position (z-coordinate)
-    target_pose.pose.orientation.x = 0.0  # Set the orientation (x-coordinate)
-    target_pose.pose.orientation.y = 0.0  # Set the orientation (y-coordinate)
-    target_pose.pose.orientation.z = 0.0  # Set the orientation (z-coordinate)
-    target_pose.pose.orientation.w = 1.0  # Set the orientation (w-coordinate)
+    target_pose.pose.position.x = pose[0]  # Set the position (x-coordinate)
+    target_pose.pose.position.y = pose[1]  # Set the position (y-coordinate)
+    target_pose.pose.position.z = pose[2]  # Set the position (z-coordinate)
+    target_pose.pose.orientation.x = pose[3]  # Set the orientation (x-coordinate)
+    target_pose.pose.orientation.y = pose[4]  # Set the orientation (y-coordinate)
+    target_pose.pose.orientation.z = pose[5]  # Set the orientation (z-coordinate)
+    target_pose.pose.orientation.w = pose[6]  # Set the orientation (w-coordinate)
 
     # Publish the target pose
     pub.publish(target_pose)

mur/mur_launch_hardware/launch/general_mur600.launch

@@ -21,9 +21,9 @@
     <arg name="ur_l_rpy" default="0.0 0.0 0.0"/>
     <arg name="ur_r_xyz" default="0.0 0.0 0.0"/>
     <arg name="ur_r_rpy" default="0.0 0.0 3.141596"/>
-    <arg name="cmplt_robot_descr" default="$(find mur_description)/urdf/mur_620.gazebo.xacro simulate_camera:= $(arg simulate_camera) use_lift:=$(arg use_lift) sim:=false tool:=$(arg tool) ur_l_rpy:='$(arg ur_l_rpy)' ur_r_rpy:='$(arg ur_r_rpy)' ur_l_xyz:='$(arg ur_l_xyz)' ur_r_xyz:='$(arg ur_r_xyz)'" /> <!-- Complete robot description -->
+    <arg name="cmplt_robot_descr" default="$(find mur_description)/urdf/mur_620.gazebo.xacro use_lift:=$(arg use_lift) tool:=$(arg tool) simulate_camera:=$(arg simulate_camera) sim:=false ur_l_rpy:='$(arg ur_l_rpy)' ur_r_rpy:='$(arg ur_r_rpy)' ur_l_xyz:='$(arg ur_l_xyz)' ur_r_xyz:='$(arg ur_r_xyz)'" /> <!-- Complete robot description -->
     <arg name="localization_type" default="mocap" /> <!-- mocap or amcl or robot_pose or odom-->
-   
+
     <!-- Bringup the base of the mur -->
 
     <!-- TODO: disable robot_state_publisher of mir if joint states are used by other publisher. else: remap robot_description? -->
@@ -112,6 +112,14 @@
                 <param name="UR_base_link_name" value="$(arg tf_prefix)/$(arg left_arm_group_name)/base_link_inertia" />
                 <param name="local_TCP_pose_topic" value="ur_calibrated_pose" />
             </node>
+            <!-- TODO: change, but has to be tested first! -->
+            <!-- <include file="$(find mur_control)/launch/global_tcp_pose_publisher.launch">
+                <arg name="world_T_base_topic" default="/$(arg tf_prefix)/mir_pose_stamped_simple"/>
+                <arg name="base_T_ee_topic" value="ur_calibrated_pose"/>
+                <arg name="world_T_ee_topic" value="global_tcp_pose"/>
+                <arg name="static_tf_parent" value="$(arg tf_prefix)/base_footprint"/>
+                <arg name="static_tf_child" value="$(arg tf_prefix)/$(arg left_arm_group_name)/base_link"/>
+            </include> -->
 
 
         </group>

mur/mur_launch_hardware/launch/mur620a.launch

@@ -39,5 +39,9 @@
         <arg name="ur_r_rpy" value="$(arg ur_r_rpy)"/>
     </include>
 
+    <!-- static transform_publisher node to complete the tf-tree -->
+    <node pkg="tf" type="static_transform_publisher" name="dummy_lift_$(arg tf_prefix)_$(arg left_arm_group_name)" args="0 0 0 0 0 0 $(arg tf_prefix)/left_lift_bottom $(arg tf_prefix)/left_lift_top 100" />
+    <node pkg="tf" type="static_transform_publisher" name="dummy_lift_$(arg tf_prefix)_$(arg right_arm_group_name)" args="0 0 0 0 0 0 $(arg tf_prefix)/right_lift_bottom $(arg tf_prefix)/right_lift_top 100" />
+
 
 </launch>

mur/mur_launch_hardware/launch/mur620b.launch

@@ -34,7 +34,14 @@
         <arg name="kinematic_config_l" value="$(find ur_launch_hardware)/calibration/calibration_UR10_19.yaml"/>
         <arg name="kinematic_config_r" value="$(find ur_launch_hardware)/calibration/calibration_UR10_17.yaml"/>
         <arg name="localization_type" value="$(arg localization_type)"/>
+        <arg name="ur_l_xyz" value="$(arg ur_l_xyz)"/>
+        <arg name="ur_l_rpy" value="$(arg ur_l_rpy)"/>
+        <arg name="ur_r_xyz" value="$(arg ur_r_xyz)"/>
+        <arg name="ur_r_rpy" value="$(arg ur_r_rpy)"/>
     </include>
 
+    <!-- static transform_publisher node to complete the tf-tree -->
+    <node pkg="tf" type="static_transform_publisher" name="dummy_lift_$(arg tf_prefix)_$(arg left_arm_group_name)" args="0 0 0 0 0 0 $(arg tf_prefix)/left_lift_bottom $(arg tf_prefix)/left_lift_top 100" />
+    <node pkg="tf" type="static_transform_publisher" name="dummy_lift_$(arg tf_prefix)_$(arg right_arm_group_name)" args="0 0 0 0 0 0 $(arg tf_prefix)/right_lift_bottom $(arg tf_prefix)/right_lift_top 100" />
 
 </launch>

mur/mur_launch_sim/launch/mur_620.launch

@@ -96,10 +96,14 @@
 				<param name="ur_prefix" value="$(arg prefix)_l/" />
 				<remap from="joint_states" to="/$(arg mur_ns)/joint_states" />
 			</node>
-			<node name="global_tcp_pose_publisher" pkg="mur_control" type="global_tcp_pose_publisher.py" output="screen">
-				<param name="UR_base_link_name" value="$(arg mur_ns)/$(arg prefix)_l/base_link" />
-				<param name="local_TCP_pose_topic" value="/$(arg mur_ns)/$(arg prefix)_l/tcp_pose" />
-			</node>
+
+			<include file="$(find mur_control)/launch/global_tcp_pose_publisher.launch">
+				<arg name="world_T_base_topic" default="/$(arg tf_prefix)/mir_pose_stamped_simple"/>
+                <arg name="base_T_ee_topic" value="tcp_pose"/>
+                <arg name="world_T_ee_topic" value="global_tcp_pose"/>
+                <arg name="static_tf_parent" value="$(arg tf_prefix)/base_footprint"/>
+                <arg name="static_tf_child" value="$(arg tf_prefix)/$(arg prefix)_l/base_link"/>
+            </include>
 		</group>
 
 
@@ -114,14 +118,17 @@
 				<param name="ur_prefix" value="$(arg prefix)_r/" />
 				<remap from="joint_states" to="/$(arg mur_ns)/joint_states" />
 			</node>
-			<node name="global_tcp_pose_publisher" pkg="mur_control" type="global_tcp_pose_publisher.py" output="screen">
-				<param name="UR_base_link_name" value="$(arg mur_ns)/$(arg prefix)_r/base_link" />
-				<param name="local_TCP_pose_topic" value="/$(arg mur_ns)/$(arg prefix)_r/tcp_pose" />
-			</node>
+			<include file="$(find mur_control)/launch/global_tcp_pose_publisher.launch">
+				<arg name="world_T_base_topic" default="/$(arg tf_prefix)/mir_pose_stamped_simple"/>
+                <arg name="base_T_ee_topic" value="tcp_pose"/>
+                <arg name="world_T_ee_topic" value="global_tcp_pose"/>
+                <arg name="static_tf_parent" value="$(arg tf_prefix)/base_footprint"/>
+                <arg name="static_tf_child" value="$(arg tf_prefix)/$(arg prefix)_r/base_link"/>
+            </include>
 		</group>
 
 		<!-- use feedback instead (ur_controllers_match twist_controller_fb )-->
-		<group if = "$(arg use_inverse_differential_kinematics)">
+		<!-- <group if = "$(arg use_inverse_differential_kinematics)">
 			<node name="twist_controller_sim_r" pkg="mur_control" type="twist_controller_sim.py" output="screen" launch-prefix="bash -c 'sleep $(arg node_start_delay); $0 $@' " >
 				<param name="robot_name" value="$(arg mur_ns)" />
 				<param name="group_vel_controller_topic" value="joint_group_vel_controller_r/unsafe/command" />
@@ -135,7 +142,7 @@
 				<param name="commanded_twist_topic" value="$(arg prefix)_l/commanded_twist" />
 				<param name="move_group_name" value="UR_arm_l" />
 			</node>
-		</group>
+		</group> -->