From df6755899e25acca4900ae5880a725d5d7b10d2f Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Mon, 27 May 2024 19:08:16 +0200 Subject: [PATCH 01/56] Added file to send references as JointTrajectory to ros_control, slightly change in the stack --- python/ros_control_bridge.py | 119 +++++++++++++++++++++++++++++++++++ stack/tiago_dual.stack | 8 ++- 2 files changed, 126 insertions(+), 1 deletion(-) create mode 100644 python/ros_control_bridge.py diff --git a/python/ros_control_bridge.py b/python/ros_control_bridge.py new file mode 100644 index 0000000..e80066d --- /dev/null +++ b/python/ros_control_bridge.py @@ -0,0 +1,119 @@ +import rospy +from sensor_msgs.msg import JointState +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from geometry_msgs.msg import Twist + +head_cmd_pub = None +head_cmd_msg = JointTrajectory() + +left_arm_cmd_pub = None +left_arm_cmd_msg = JointTrajectory() + +right_arm_cmd_pub = None +right_arm_cmd_msg = JointTrajectory() + +torso_cmd_pub = None +torso_cmd_msg = JointTrajectory() + +mobile_base_cmd_pub = None +mobile_base_cmd_msg = Twist() + +rate_ = 10. +time_duration = rospy.Time() + +SEND_VELOCITY = False + +def set_initial_configuration(data: JointState): + home = dict() + home["reference@v0"] = 0.0 + home["reference@v1"] = 0.0 + home["reference@v2"] = 0.0 + home["reference@v3"] = 0.0 + home["reference@v4"] = 0.0 + home["reference@v5"] = 0.0 + + for i in range(len(data.name)): + home[data.name[i]] = data.position[i] + + print("Initial configuration:") + for key in home: + print(f"{key}: {home[key]}") + + rospy.set_param("cartesian/home", home) + +def fill_cms_msg(data:JointState, cmd_msg: JointTrajectory, rate: float, send_velocity = True): + for joint_name in cmd_msg.joint_names: + cmd_msg.points[0].positions[cmd_msg.joint_names.index(joint_name)] = data.position[data.name.index(joint_name)] + if send_velocity: + cmd_msg.points[0].velocities[cmd_msg.joint_names.index(joint_name)] = data.velocity[data.name.index(joint_name)] + cmd_msg.points[0].time_from_start = rospy.Duration(1.) + return cmd_msg +def io_callback(data: JointState): + global head_cmd_pub, head_cmd_msg + global left_arm_cmd_pub, left_arm_cmd_msg + global right_arm_cmd_pub, right_arm_cmd_msg + global torso_cmd_pub, torso_cmd_msg + global mobile_base_cmd_pub, mobile_base_cmd_msg + global rate_, time_duration + + head_cmd_msg = fill_cms_msg(data, head_cmd_msg, rate_, send_velocity=SEND_VELOCITY) + left_arm_cmd_msg = fill_cms_msg(data, left_arm_cmd_msg, rate_, send_velocity=SEND_VELOCITY) + right_arm_cmd_msg = fill_cms_msg(data, right_arm_cmd_msg, rate_, send_velocity=SEND_VELOCITY) + torso_cmd_msg = fill_cms_msg(data, torso_cmd_msg, rate_, send_velocity=SEND_VELOCITY) + + mobile_base_cmd_msg.linear.x = data.velocity[0] + mobile_base_cmd_msg.linear.y = data.velocity[1] + mobile_base_cmd_msg.linear.z = 0. + mobile_base_cmd_msg.angular.x = 0. + mobile_base_cmd_msg.angular.y = 0. + mobile_base_cmd_msg.angular.z = data.velocity[5] + + time = rospy.Time.now() + + head_cmd_msg.header.stamp = time + left_arm_cmd_msg.header.stamp = time + right_arm_cmd_msg.header.stamp = time + torso_cmd_msg.header.stamp = time + + head_cmd_pub.publish(head_cmd_msg) + left_arm_cmd_pub.publish(left_arm_cmd_msg) + right_arm_cmd_pub.publish(right_arm_cmd_msg) + torso_cmd_pub.publish(torso_cmd_msg) + mobile_base_cmd_pub.publish(mobile_base_cmd_msg) + +def init_cmd_msg(cmd_msg: JointTrajectory, joint_names): + cmd_msg.joint_names = joint_names + cmd_msg.points.append(JointTrajectoryPoint()) + cmd_msg.points[0].positions = [0.] * len(cmd_msg.joint_names) + cmd_msg.points[0].velocities = [0.] * len(cmd_msg.joint_names) + return cmd_msg + +if __name__ == '__main__': + rospy.init_node('ros_control_bridge', anonymous=True) + time_duration = rospy.get_rostime() + print(f"SEND_VELOCITY: {SEND_VELOCITY}") + + data = rospy.wait_for_message("joint_states", JointState, timeout=5) + set_initial_configuration(data) + + head_cmd_pub = rospy.Publisher('head_controller/command', JointTrajectory, queue_size=1) + head_cmd_msg = init_cmd_msg(head_cmd_msg, ["head_1_joint", "head_2_joint"]) + + left_arm_cmd_pub = rospy.Publisher('/arm_left_controller/command', JointTrajectory, queue_size=1) + left_arm_cmd_msg = init_cmd_msg(left_arm_cmd_msg, ["arm_left_1_joint", "arm_left_2_joint", "arm_left_3_joint", "arm_left_4_joint", + "arm_left_5_joint", "arm_left_6_joint", "arm_left_7_joint"]) + + right_arm_cmd_pub = rospy.Publisher('/arm_right_controller/command', JointTrajectory, queue_size=1) + right_arm_cmd_msg = init_cmd_msg(right_arm_cmd_msg, ["arm_right_1_joint", "arm_right_2_joint", "arm_right_3_joint", "arm_right_4_joint", + "arm_right_5_joint", "arm_right_6_joint", "arm_right_7_joint"]) + + torso_cmd_pub = rospy.Publisher('/torso_controller/command', JointTrajectory, queue_size=1) + torso_cmd_msg = init_cmd_msg(torso_cmd_msg, ["torso_lift_joint"]) + + mobile_base_cmd_pub = rospy.Publisher('/mobile_base_controller/cmd_vel', Twist, queue_size=1) + + rospy.Subscriber("cartesian/solution", JointState, io_callback) + + rate = rospy.Rate(rate_) + while not rospy.is_shutdown(): + rate.sleep() \ No newline at end of file diff --git a/stack/tiago_dual.stack b/stack/tiago_dual.stack index 8dd9e76..64f3403 100644 --- a/stack/tiago_dual.stack +++ b/stack/tiago_dual.stack @@ -3,7 +3,8 @@ solver_options: back_end: "qpoases" stack: - - ["Lee", "Ree", "Base", "Gaze"] + - ["Gaze"] + - ["Lee", "Ree", "Base"] - ["Postural"] @@ -139,5 +140,10 @@ Collision: - [gripper_right_link, arm_left_4_link] - [gripper_right_link, arm_left_5_link] + - [torso_fixed_column_link, gripper_right_left_finger_link] + - [torso_fixed_column_link, gripper_right_right_finger_link] + - [torso_fixed_column_link, gripper_left_left_finger_link] + - [torso_fixed_column_link, gripper_left_right_finger_link] + From 6aacb61fbd1b03f0419876d90f4c850a506bc47c Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Tue, 28 May 2024 20:38:05 +0200 Subject: [PATCH 02/56] added trasformation from /map to /ci/world --- launch/cartesio.launch | 3 +++ 1 file changed, 3 insertions(+) diff --git a/launch/cartesio.launch b/launch/cartesio.launch index d1ed60d..816130b 100644 --- a/launch/cartesio.launch +++ b/launch/cartesio.launch @@ -57,6 +57,9 @@ + + + From 76f94f1af2f64639abbdd3762fae4d33b145fcb8 Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Tue, 28 May 2024 20:38:40 +0200 Subject: [PATCH 03/56] Added code to run on the real robot --- python/ros_control_bridge.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/python/ros_control_bridge.py b/python/ros_control_bridge.py index e80066d..2d28564 100644 --- a/python/ros_control_bridge.py +++ b/python/ros_control_bridge.py @@ -19,7 +19,7 @@ mobile_base_cmd_msg = Twist() rate_ = 10. -time_duration = rospy.Time() +time_duration = 1.0 SEND_VELOCITY = False @@ -42,11 +42,12 @@ def set_initial_configuration(data: JointState): rospy.set_param("cartesian/home", home) def fill_cms_msg(data:JointState, cmd_msg: JointTrajectory, rate: float, send_velocity = True): + global time_duration for joint_name in cmd_msg.joint_names: cmd_msg.points[0].positions[cmd_msg.joint_names.index(joint_name)] = data.position[data.name.index(joint_name)] if send_velocity: cmd_msg.points[0].velocities[cmd_msg.joint_names.index(joint_name)] = data.velocity[data.name.index(joint_name)] - cmd_msg.points[0].time_from_start = rospy.Duration(1.) + cmd_msg.points[0].time_from_start = rospy.Duration(time_duration) return cmd_msg def io_callback(data: JointState): global head_cmd_pub, head_cmd_msg @@ -68,14 +69,14 @@ def io_callback(data: JointState): mobile_base_cmd_msg.angular.y = 0. mobile_base_cmd_msg.angular.z = data.velocity[5] - time = rospy.Time.now() + time = rospy.get_rostime() #rospy.Time.now() head_cmd_msg.header.stamp = time left_arm_cmd_msg.header.stamp = time right_arm_cmd_msg.header.stamp = time torso_cmd_msg.header.stamp = time - head_cmd_pub.publish(head_cmd_msg) + #head_cmd_pub.publish(head_cmd_msg) left_arm_cmd_pub.publish(left_arm_cmd_msg) right_arm_cmd_pub.publish(right_arm_cmd_msg) torso_cmd_pub.publish(torso_cmd_msg) @@ -90,7 +91,6 @@ def init_cmd_msg(cmd_msg: JointTrajectory, joint_names): if __name__ == '__main__': rospy.init_node('ros_control_bridge', anonymous=True) - time_duration = rospy.get_rostime() print(f"SEND_VELOCITY: {SEND_VELOCITY}") data = rospy.wait_for_message("joint_states", JointState, timeout=5) From bb55aa1c09f046ce738a2366fbe10fa8e815f3aa Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Tue, 28 May 2024 20:38:58 +0200 Subject: [PATCH 04/56] added bridge from teleoperation device to cartesio --- python/teleop_bridge.py | 44 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 python/teleop_bridge.py diff --git a/python/teleop_bridge.py b/python/teleop_bridge.py new file mode 100644 index 0000000..3b5bfed --- /dev/null +++ b/python/teleop_bridge.py @@ -0,0 +1,44 @@ +#!/usr/bin/env python + +from cartesian_interface.pyci_all import * +import rospy +from geometry_msgs.msg import PoseStamped +from scipy.spatial.transform import Rotation as R +import numpy as np + +ci = pyci.CartesianInterfaceRos() +ee_left_pose_initial, _, _ = ci.getPoseReference('gripper_left_grasping_frame') +reference_publisher = rospy.Publisher('/cartesian/gripper_left_grasping_frame/reference', PoseStamped, queue_size=10) +initial_ref = None +def callback(data: PoseStamped): + r_init = R.from_quat([initial_ref.pose.orientation.x, initial_ref.pose.orientation.y, initial_ref.pose.orientation.z, initial_ref.pose.orientation.w]) + data_m_init = Affine3() + data_m_init.translation = np.array([initial_ref.pose.position.x, initial_ref.pose.position.y, initial_ref.pose.position.z]) + data_m_init.linear = r_init.as_matrix() + + r = R.from_quat([data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w]) + data_m = Affine3() + data_m.translation = np.array([data.pose.position.x, data.pose.position.y, data.pose.position.z]) + data_m.linear = r.as_matrix() + + ref_m = ee_left_pose_initial * data_m_init.inverse() * data_m + + reference = PoseStamped() + reference.pose.position.x = ref_m.translation[0] + reference.pose.position.y = ref_m.translation[1] + reference.pose.position.z = ref_m.translation[2] + reference.pose.orientation.x = data.pose.orientation.x + reference.pose.orientation.y = data.pose.orientation.y + reference.pose.orientation.z = data.pose.orientation.z + reference.pose.orientation.w = data.pose.orientation.w + + reference.header.frame_id = "ci/world" + reference.header.stamp = rospy.get_rostime() + reference_publisher.publish(reference) + +if __name__ == '__main__': + rospy.init_node('teleop_bridge', anonymous=True) + initial_ref = rospy.wait_for_message("pos", PoseStamped, timeout=5) + rospy.Subscriber("pos", PoseStamped, callback) + + rospy.spin() \ No newline at end of file From 4bd5359d3fff62d34fe245cfbe25c34e4e294cc5 Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Wed, 29 May 2024 20:51:07 +0200 Subject: [PATCH 05/56] Cartesian tasks for the arms are in base_link --- stack/tiago_dual.stack | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/stack/tiago_dual.stack b/stack/tiago_dual.stack index 64f3403..eb30637 100644 --- a/stack/tiago_dual.stack +++ b/stack/tiago_dual.stack @@ -22,13 +22,13 @@ Omni4X: Lee: type: "Cartesian" distal_link: "gripper_left_grasping_frame" - base_link: "world" + base_link: "base_link" lambda: 0.1 Ree: type: "Cartesian" distal_link: "gripper_right_grasping_frame" - base_link: "world" + base_link: "base_link" lambda: 0.1 Gaze: From 07e02ab09a99182ad63c4ad61c799cb95fb13bf4 Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Wed, 29 May 2024 20:51:50 +0200 Subject: [PATCH 06/56] Added launch file for teleop.py --- launch/teleop.launch | 7 ++++ python/ros_control_bridge.py | 6 +-- python/teleop_bridge.py | 76 ++++++++++++++++++++++-------------- 3 files changed, 56 insertions(+), 33 deletions(-) create mode 100644 launch/teleop.launch mode change 100644 => 100755 python/teleop_bridge.py diff --git a/launch/teleop.launch b/launch/teleop.launch new file mode 100644 index 0000000..5f3fd7c --- /dev/null +++ b/launch/teleop.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/python/ros_control_bridge.py b/python/ros_control_bridge.py index 2d28564..cd8f2d9 100644 --- a/python/ros_control_bridge.py +++ b/python/ros_control_bridge.py @@ -18,8 +18,8 @@ mobile_base_cmd_pub = None mobile_base_cmd_msg = Twist() -rate_ = 10. -time_duration = 1.0 +rate_ = 50. #10. +time_duration = 0.2 #1.0 SEND_VELOCITY = False @@ -116,4 +116,4 @@ def init_cmd_msg(cmd_msg: JointTrajectory, joint_names): rate = rospy.Rate(rate_) while not rospy.is_shutdown(): - rate.sleep() \ No newline at end of file + rate.sleep() diff --git a/python/teleop_bridge.py b/python/teleop_bridge.py old mode 100644 new mode 100755 index 3b5bfed..2620b62 --- a/python/teleop_bridge.py +++ b/python/teleop_bridge.py @@ -7,38 +7,54 @@ import numpy as np ci = pyci.CartesianInterfaceRos() -ee_left_pose_initial, _, _ = ci.getPoseReference('gripper_left_grasping_frame') -reference_publisher = rospy.Publisher('/cartesian/gripper_left_grasping_frame/reference', PoseStamped, queue_size=10) -initial_ref = None +pose_initial = Affine3() +initial_ref = PoseStamped() +initialized = False +controlled_frame = str() + def callback(data: PoseStamped): - r_init = R.from_quat([initial_ref.pose.orientation.x, initial_ref.pose.orientation.y, initial_ref.pose.orientation.z, initial_ref.pose.orientation.w]) - data_m_init = Affine3() - data_m_init.translation = np.array([initial_ref.pose.position.x, initial_ref.pose.position.y, initial_ref.pose.position.z]) - data_m_init.linear = r_init.as_matrix() - - r = R.from_quat([data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w]) - data_m = Affine3() - data_m.translation = np.array([data.pose.position.x, data.pose.position.y, data.pose.position.z]) - data_m.linear = r.as_matrix() - - ref_m = ee_left_pose_initial * data_m_init.inverse() * data_m - - reference = PoseStamped() - reference.pose.position.x = ref_m.translation[0] - reference.pose.position.y = ref_m.translation[1] - reference.pose.position.z = ref_m.translation[2] - reference.pose.orientation.x = data.pose.orientation.x - reference.pose.orientation.y = data.pose.orientation.y - reference.pose.orientation.z = data.pose.orientation.z - reference.pose.orientation.w = data.pose.orientation.w - - reference.header.frame_id = "ci/world" - reference.header.stamp = rospy.get_rostime() - reference_publisher.publish(reference) + global initial_ref, initialized, controlled_frame, pose_initial, ci, pose_initial + + if not initialized: + initial_ref = data + pose_initial, _, _ = ci.getPoseReference(controlled_frame) + initialized = True + rospy.loginfo("correclty initialized") + else: + r_init = R.from_quat( + [initial_ref.pose.orientation.x, initial_ref.pose.orientation.y, initial_ref.pose.orientation.z, + initial_ref.pose.orientation.w]) + data_m_init = Affine3() + data_m_init.translation = np.array( + [initial_ref.pose.position.x, initial_ref.pose.position.y, initial_ref.pose.position.z]) + data_m_init.linear = r_init.as_matrix() + + r = R.from_quat( + [data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w]) + data_m = Affine3() + data_m.translation = np.array([data.pose.position.x, data.pose.position.y, data.pose.position.z]) + data_m.linear = r.as_matrix() + + ref_m = pose_initial * data_m_init.inverse() * data_m + + pose_ref = Affine3() + pose_ref.translation = ref_m.translation + pose_ref.linear = data_m.linear + + ci.setPoseReference(controlled_frame, pose_ref) + if __name__ == '__main__': rospy.init_node('teleop_bridge', anonymous=True) - initial_ref = rospy.wait_for_message("pos", PoseStamped, timeout=5) - rospy.Subscriber("pos", PoseStamped, callback) - rospy.spin() \ No newline at end of file + if rospy.has_param('~controlled_frame'): + controlled_frame = rospy.get_param('~controlled_frame') + else: + rospy.logerr("controlled_frame private param is mandatory! Exiting.") + exit() + + rospy.loginfo(f"controlled_frame: {controlled_frame}") + + rospy.Subscriber("teleop_pose", PoseStamped, callback) + + rospy.spin() From 2f9577a579dd4a393a03245c3e5a3fb3745dc53c Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Thu, 30 May 2024 12:10:37 +0200 Subject: [PATCH 07/56] Added python script for teleoperate the gripper --- launch/teleop.launch | 20 ++++++++++++++++++-- python/ros_control_bridge.py | 2 ++ python/teleop_bridge.py | 2 +- 3 files changed, 21 insertions(+), 3 deletions(-) mode change 100644 => 100755 python/ros_control_bridge.py diff --git a/launch/teleop.launch b/launch/teleop.launch index 5f3fd7c..38b5a6b 100644 --- a/launch/teleop.launch +++ b/launch/teleop.launch @@ -1,7 +1,23 @@ - - + + + + + + + + + + + + + $(arg controller_gripper_joints) + + Ù + + + diff --git a/python/ros_control_bridge.py b/python/ros_control_bridge.py old mode 100644 new mode 100755 index cd8f2d9..e4cb8f4 --- a/python/ros_control_bridge.py +++ b/python/ros_control_bridge.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + import rospy from sensor_msgs.msg import JointState from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint diff --git a/python/teleop_bridge.py b/python/teleop_bridge.py index 2620b62..9ac27d4 100755 --- a/python/teleop_bridge.py +++ b/python/teleop_bridge.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 from cartesian_interface.pyci_all import * import rospy From 111f0582f475e73a908aa52144071ced210b044d Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Thu, 30 May 2024 14:02:05 +0200 Subject: [PATCH 08/56] Added service to enable/disable send of commands to controllers --- python/ros_control_bridge.py | 29 +++++++++++++++++++++++------ 1 file changed, 23 insertions(+), 6 deletions(-) diff --git a/python/ros_control_bridge.py b/python/ros_control_bridge.py index e4cb8f4..6c67adb 100755 --- a/python/ros_control_bridge.py +++ b/python/ros_control_bridge.py @@ -4,6 +4,7 @@ from sensor_msgs.msg import JointState from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from geometry_msgs.msg import Twist +from std_srvs.srv import SetBool, SetBoolResponse head_cmd_pub = None head_cmd_msg = JointTrajectory() @@ -23,6 +24,8 @@ rate_ = 50. #10. time_duration = 0.2 #1.0 +send_commands = True + SEND_VELOCITY = False def set_initial_configuration(data: JointState): @@ -58,6 +61,7 @@ def io_callback(data: JointState): global torso_cmd_pub, torso_cmd_msg global mobile_base_cmd_pub, mobile_base_cmd_msg global rate_, time_duration + global send_commands head_cmd_msg = fill_cms_msg(data, head_cmd_msg, rate_, send_velocity=SEND_VELOCITY) left_arm_cmd_msg = fill_cms_msg(data, left_arm_cmd_msg, rate_, send_velocity=SEND_VELOCITY) @@ -71,18 +75,19 @@ def io_callback(data: JointState): mobile_base_cmd_msg.angular.y = 0. mobile_base_cmd_msg.angular.z = data.velocity[5] - time = rospy.get_rostime() #rospy.Time.now() + time = rospy.get_rostime() head_cmd_msg.header.stamp = time left_arm_cmd_msg.header.stamp = time right_arm_cmd_msg.header.stamp = time torso_cmd_msg.header.stamp = time - #head_cmd_pub.publish(head_cmd_msg) - left_arm_cmd_pub.publish(left_arm_cmd_msg) - right_arm_cmd_pub.publish(right_arm_cmd_msg) - torso_cmd_pub.publish(torso_cmd_msg) - mobile_base_cmd_pub.publish(mobile_base_cmd_msg) + if send_commands: + head_cmd_pub.publish(head_cmd_msg) + left_arm_cmd_pub.publish(left_arm_cmd_msg) + right_arm_cmd_pub.publish(right_arm_cmd_msg) + torso_cmd_pub.publish(torso_cmd_msg) + mobile_base_cmd_pub.publish(mobile_base_cmd_msg) def init_cmd_msg(cmd_msg: JointTrajectory, joint_names): cmd_msg.joint_names = joint_names @@ -91,6 +96,14 @@ def init_cmd_msg(cmd_msg: JointTrajectory, joint_names): cmd_msg.points[0].velocities = [0.] * len(cmd_msg.joint_names) return cmd_msg +def set_send_commands(req: SetBool): + global send_commands + send_commands = req.data + response = SetBoolResponse() + response.success = True + response.message = "send_response set to " + str(req.data) + return response + if __name__ == '__main__': rospy.init_node('ros_control_bridge', anonymous=True) print(f"SEND_VELOCITY: {SEND_VELOCITY}") @@ -114,6 +127,10 @@ def init_cmd_msg(cmd_msg: JointTrajectory, joint_names): mobile_base_cmd_pub = rospy.Publisher('/mobile_base_controller/cmd_vel', Twist, queue_size=1) + if rospy.has_param('~send_commands'): + send_commands=rospy.get_param('~send_commands') + rospy.Service('send_commands', SetBool, set_send_commands) + rospy.Subscriber("cartesian/solution", JointState, io_callback) rate = rospy.Rate(rate_) From 06772dd58400d7c474d05e73e4b3e35a5fb7ffae Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Thu, 30 May 2024 14:02:24 +0200 Subject: [PATCH 09/56] Added missing gripper script --- python/teleop_gripper_bridge.py | 84 +++++++++++++++++++++++++++++++++ 1 file changed, 84 insertions(+) create mode 100755 python/teleop_gripper_bridge.py diff --git a/python/teleop_gripper_bridge.py b/python/teleop_gripper_bridge.py new file mode 100755 index 0000000..a35a224 --- /dev/null +++ b/python/teleop_gripper_bridge.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 + +import rospy +from std_msgs.msg import Float32 +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + +gripper_cmd_pub = None +gripper_cmd_msg = JointTrajectory() + +GRIPPER_MAX = 0.044 +GRIPPER_MIN = 0.010 + +TELEOP_MAX = 0. +TELEOP_MIN = 1. + +rate_ = 50. +time_duration = 0.2 + +def print_info(): + rospy.loginfo(f" GRIPPER_MAX: {GRIPPER_MAX}") + rospy.loginfo(f" GRIPPER_MIN: {GRIPPER_MIN}") + rospy.loginfo(f" TELEOP_MAX: {TELEOP_MAX}") + rospy.loginfo(f" TELEOP_MIN: {TELEOP_MIN}") + +def io_callback(data: Float32): + a = (GRIPPER_MAX - GRIPPER_MIN)/(TELEOP_MAX-TELEOP_MIN) + d = data.data + gripper = a * d * (d - TELEOP_MAX) + GRIPPER_MAX + + for joint_name in gripper_cmd_msg.joint_names: + gripper_cmd_msg.points[0].positions[gripper_cmd_msg.joint_names.index(joint_name)] = gripper + gripper_cmd_msg.points[0].time_from_start = rospy.Duration(time_duration) + + gripper_cmd_pub.publish(gripper_cmd_msg) + +def init_cmd_msg(cmd_msg: JointTrajectory, joint_names): + cmd_msg.joint_names = joint_names + cmd_msg.points.append(JointTrajectoryPoint()) + cmd_msg.points[0].positions = [0.] * len(cmd_msg.joint_names) + cmd_msg.points[0].velocities = [0.] * len(cmd_msg.joint_names) + return cmd_msg + +if __name__ == '__main__': + rospy.init_node('ros_control_gripper_bridge', anonymous=True) + + controller_gripper = str() + if rospy.has_param('~controller_gripper'): + controller_gripper = rospy.get_param('~controller_gripper') + else: + rospy.logerr("controller_gripper private param is mandatory! Exiting.") + exit() + rospy.loginfo(f"controller_gripper: {controller_gripper}") + + controller_gripper_joints = [] + if rospy.has_param('~controller_gripper_joints'): + controller_gripper_joints = rospy.get_param('~controller_gripper_joints') + else: + rospy.logerr("controller_gripper_joints private param is mandatory! Exiting.") + exit() + rospy.loginfo(f"controller_gripper_joints: {controller_gripper_joints}") + + if rospy.has_param('~GRIPPER_MAX'): + GRIPPER_MAX = rospy.get_param('~GRIPPER_MAX') + if rospy.has_param('~GRIPPER_MIN'): + GRIPPER_MIN = rospy.get_param('~GRIPPER_MIN') + if rospy.has_param('~TELEOP_MAX'): + TELEOP_MAX = rospy.get_param('~TELEOP_MAX') + if rospy.has_param('~TELEOP_MIN'): + TELEOP_MIN = rospy.get_param('~TELEOP_MIN') + + print_info() + + gripper_cmd_pub = rospy.Publisher(controller_gripper + '/command', JointTrajectory, queue_size=1) + gripper_cmd_msg = init_cmd_msg(gripper_cmd_msg, controller_gripper_joints) + + rospy.Subscriber("teleop_gripper", Float32, io_callback) + + rate = rospy.Rate(rate_) + while not rospy.is_shutdown(): + rate.sleep() + + + + From 93ff2362a8d4ca11704b08de775eec57bcbfc85a Mon Sep 17 00:00:00 2001 From: Enrico Mingo Hoffman Date: Thu, 30 May 2024 15:59:20 +0200 Subject: [PATCH 10/56] Update README.md --- README.md | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/README.md b/README.md index 2327685..907f62f 100644 --- a/README.md +++ b/README.md @@ -17,4 +17,25 @@ Simply: It is possible to check the collision model by enabling ```Collision Enabled``` in the ```RobotModel``` display in RVIZ. Notice that different collision models can be used. +How to run on the robot +----------------------- +To run the controller on the robot first run the ```ros_control_bridge.py```: + +```rosrun tiago_dual_cartesio_config ros_control_bridge.py``` + +this will forward the solution from the controller to the ```JointTrajectory``` controllers running on the robot. + +Then run the controller using: + +```reset && mon launch tiago_dual_cartesio_config/launch/cartesio.launch``` + +Connect to Teleoperation device +------------------------------- +First, perform the steps in **How to run on the robot**. Then run the launch file: + +```reset && mon launch tiago_dual_cartesio_config teleop.launch``` + +Notice that the ```teleop.launch``` contains two Python nodes that can be replicated in order to control both left and right arms and grippers. + + From bc5e39b9f2c6dbbf5c234236577c0adba4832e34 Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Thu, 30 May 2024 19:09:19 +0200 Subject: [PATCH 11/56] set to false inertia weight for the postural task --- stack/tiago_dual.stack | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stack/tiago_dual.stack b/stack/tiago_dual.stack index eb30637..f643ebe 100644 --- a/stack/tiago_dual.stack +++ b/stack/tiago_dual.stack @@ -40,7 +40,7 @@ Gaze: Postural: type: "Postural" lambda: 0.1 - use_inertia: true + use_inertia: false weight: reference@v0: 0.0 reference@v1: 0.0 From 3f4e275db425e708291ec392e523e47b4e97e092 Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Thu, 30 May 2024 19:10:29 +0200 Subject: [PATCH 12/56] Added camera frame --- capsules/urdf/tiago_dual_capsules.rviz | 7 +++++++ python/ros_control_bridge.py | 1 + robots/tiago_dual.urdf.xacro | 8 ++++++++ 3 files changed, 16 insertions(+) diff --git a/capsules/urdf/tiago_dual_capsules.rviz b/capsules/urdf/tiago_dual_capsules.rviz index 577dc26..e282fb5 100644 --- a/capsules/urdf/tiago_dual_capsules.rviz +++ b/capsules/urdf/tiago_dual_capsules.rviz @@ -2341,4 +2341,11 @@ + + + + + + + diff --git a/python/ros_control_bridge.py b/python/ros_control_bridge.py index 6c67adb..fd5955b 100755 --- a/python/ros_control_bridge.py +++ b/python/ros_control_bridge.py @@ -130,6 +130,7 @@ def set_send_commands(req: SetBool): if rospy.has_param('~send_commands'): send_commands=rospy.get_param('~send_commands') rospy.Service('send_commands', SetBool, set_send_commands) + print(f"send_commands: {send_commands}") rospy.Subscriber("cartesian/solution", JointState, io_callback) diff --git a/robots/tiago_dual.urdf.xacro b/robots/tiago_dual.urdf.xacro index d74c79d..364bcc4 100644 --- a/robots/tiago_dual.urdf.xacro +++ b/robots/tiago_dual.urdf.xacro @@ -10,5 +10,13 @@ + + + + + + + + From c1f15763a1dc8bfd1d489f62a94e143761130a0c Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Fri, 31 May 2024 15:35:16 +0200 Subject: [PATCH 13/56] Fixed bug in initialization --- python/teleop_bridge.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/python/teleop_bridge.py b/python/teleop_bridge.py index 9ac27d4..5822f22 100755 --- a/python/teleop_bridge.py +++ b/python/teleop_bridge.py @@ -17,7 +17,8 @@ def callback(data: PoseStamped): if not initialized: initial_ref = data - pose_initial, _, _ = ci.getPoseReference(controlled_frame) + tmp, _, _ = ci.getPoseReference(controlled_frame) + pose_initial.translation = tmp.translation initialized = True rospy.loginfo("correclty initialized") else: From d3247500d14136a318d33bfd7626816bb053fcaa Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Mon, 23 Sep 2024 14:38:24 +0200 Subject: [PATCH 14/56] Modified ros ctrl bridge & stack --- python/ros_control_bridge.py | 143 +++++++++++++++++++++++++---------- stack/tiago_dual.stack | 16 +++- 2 files changed, 118 insertions(+), 41 deletions(-) mode change 100755 => 100644 python/ros_control_bridge.py diff --git a/python/ros_control_bridge.py b/python/ros_control_bridge.py old mode 100755 new mode 100644 index fd5955b..cce8366 --- a/python/ros_control_bridge.py +++ b/python/ros_control_bridge.py @@ -4,6 +4,7 @@ from sensor_msgs.msg import JointState from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from geometry_msgs.msg import Twist +from std_msgs.msg import Float32 from std_srvs.srv import SetBool, SetBoolResponse head_cmd_pub = None @@ -21,13 +22,13 @@ mobile_base_cmd_pub = None mobile_base_cmd_msg = Twist() -rate_ = 50. #10. -time_duration = 0.2 #1.0 +time_from_start = 0.2 send_commands = True SEND_VELOCITY = False + def set_initial_configuration(data: JointState): home = dict() home["reference@v0"] = 0.0 @@ -40,39 +41,48 @@ def set_initial_configuration(data: JointState): for i in range(len(data.name)): home[data.name[i]] = data.position[i] - print("Initial configuration:") + rospy.loginfo("Initial configuration:") for key in home: - print(f"{key}: {home[key]}") + rospy.loginfo(f"{key}: {home[key]}") rospy.set_param("cartesian/home", home) -def fill_cms_msg(data:JointState, cmd_msg: JointTrajectory, rate: float, send_velocity = True): - global time_duration + +def fill_cms_msg(data: JointState, cmd_msg: JointTrajectory, send_velocity=True): + global time_from_start for joint_name in cmd_msg.joint_names: - cmd_msg.points[0].positions[cmd_msg.joint_names.index(joint_name)] = data.position[data.name.index(joint_name)] + cmd_msg.points[0].positions[cmd_msg.joint_names.index(joint_name)] = ( + data.position[data.name.index(joint_name)] + ) if send_velocity: - cmd_msg.points[0].velocities[cmd_msg.joint_names.index(joint_name)] = data.velocity[data.name.index(joint_name)] - cmd_msg.points[0].time_from_start = rospy.Duration(time_duration) + cmd_msg.points[0].velocities[cmd_msg.joint_names.index(joint_name)] = ( + data.velocity[data.name.index(joint_name)] + ) + cmd_msg.points[0].time_from_start = rospy.Duration(time_from_start) return cmd_msg + + def io_callback(data: JointState): global head_cmd_pub, head_cmd_msg global left_arm_cmd_pub, left_arm_cmd_msg global right_arm_cmd_pub, right_arm_cmd_msg global torso_cmd_pub, torso_cmd_msg global mobile_base_cmd_pub, mobile_base_cmd_msg - global rate_, time_duration + global time_from_start global send_commands - head_cmd_msg = fill_cms_msg(data, head_cmd_msg, rate_, send_velocity=SEND_VELOCITY) - left_arm_cmd_msg = fill_cms_msg(data, left_arm_cmd_msg, rate_, send_velocity=SEND_VELOCITY) - right_arm_cmd_msg = fill_cms_msg(data, right_arm_cmd_msg, rate_, send_velocity=SEND_VELOCITY) - torso_cmd_msg = fill_cms_msg(data, torso_cmd_msg, rate_, send_velocity=SEND_VELOCITY) + head_cmd_msg = fill_cms_msg(data, head_cmd_msg, send_velocity=SEND_VELOCITY) + left_arm_cmd_msg = fill_cms_msg(data, left_arm_cmd_msg, send_velocity=SEND_VELOCITY) + right_arm_cmd_msg = fill_cms_msg( + data, right_arm_cmd_msg, send_velocity=SEND_VELOCITY + ) + torso_cmd_msg = fill_cms_msg(data, torso_cmd_msg, send_velocity=SEND_VELOCITY) mobile_base_cmd_msg.linear.x = data.velocity[0] mobile_base_cmd_msg.linear.y = data.velocity[1] - mobile_base_cmd_msg.linear.z = 0. - mobile_base_cmd_msg.angular.x = 0. - mobile_base_cmd_msg.angular.y = 0. + mobile_base_cmd_msg.linear.z = 0.0 + mobile_base_cmd_msg.angular.x = 0.0 + mobile_base_cmd_msg.angular.y = 0.0 mobile_base_cmd_msg.angular.z = data.velocity[5] time = rospy.get_rostime() @@ -89,13 +99,15 @@ def io_callback(data: JointState): torso_cmd_pub.publish(torso_cmd_msg) mobile_base_cmd_pub.publish(mobile_base_cmd_msg) + def init_cmd_msg(cmd_msg: JointTrajectory, joint_names): cmd_msg.joint_names = joint_names cmd_msg.points.append(JointTrajectoryPoint()) - cmd_msg.points[0].positions = [0.] * len(cmd_msg.joint_names) - cmd_msg.points[0].velocities = [0.] * len(cmd_msg.joint_names) + cmd_msg.points[0].positions = [0.0] * len(cmd_msg.joint_names) + cmd_msg.points[0].velocities = [0.0] * len(cmd_msg.joint_names) return cmd_msg + def set_send_commands(req: SetBool): global send_commands send_commands = req.data @@ -104,36 +116,87 @@ def set_send_commands(req: SetBool): response.message = "send_response set to " + str(req.data) return response -if __name__ == '__main__': - rospy.init_node('ros_control_bridge', anonymous=True) - print(f"SEND_VELOCITY: {SEND_VELOCITY}") +def time_from_start_cb(msg: Float32): + global time_from_start + if msg.data != time_from_start: + if msg.data >= 0.1: + time_from_start = msg.data + rospy.loginfo(f"Setting time_from_start={time_from_start}") + else: + rospy.logwarn( + "Trying to set a 'time_from_start' too low. It must be >= 0.1secs" + ) + + +if __name__ == "__main__": + rospy.init_node("ros_control_bridge", anonymous=True) + rospy.loginfo(f"SEND_VELOCITY: {SEND_VELOCITY}") + + # Wait to receive the first msg from /joint_states data = rospy.wait_for_message("joint_states", JointState, timeout=5) set_initial_configuration(data) - head_cmd_pub = rospy.Publisher('head_controller/command', JointTrajectory, queue_size=1) + # Set up command publishers and msgs + head_cmd_pub = rospy.Publisher( + "head_controller/command", JointTrajectory, queue_size=1 + ) head_cmd_msg = init_cmd_msg(head_cmd_msg, ["head_1_joint", "head_2_joint"]) - left_arm_cmd_pub = rospy.Publisher('/arm_left_controller/command', JointTrajectory, queue_size=1) - left_arm_cmd_msg = init_cmd_msg(left_arm_cmd_msg, ["arm_left_1_joint", "arm_left_2_joint", "arm_left_3_joint", "arm_left_4_joint", - "arm_left_5_joint", "arm_left_6_joint", "arm_left_7_joint"]) - - right_arm_cmd_pub = rospy.Publisher('/arm_right_controller/command', JointTrajectory, queue_size=1) - right_arm_cmd_msg = init_cmd_msg(right_arm_cmd_msg, ["arm_right_1_joint", "arm_right_2_joint", "arm_right_3_joint", "arm_right_4_joint", - "arm_right_5_joint", "arm_right_6_joint", "arm_right_7_joint"]) - - torso_cmd_pub = rospy.Publisher('/torso_controller/command', JointTrajectory, queue_size=1) + left_arm_cmd_pub = rospy.Publisher( + "/arm_left_controller/command", JointTrajectory, queue_size=1 + ) + left_arm_cmd_msg = init_cmd_msg( + left_arm_cmd_msg, + [ + "arm_left_1_joint", + "arm_left_2_joint", + "arm_left_3_joint", + "arm_left_4_joint", + "arm_left_5_joint", + "arm_left_6_joint", + "arm_left_7_joint", + ], + ) + + right_arm_cmd_pub = rospy.Publisher( + "/arm_right_controller/command", JointTrajectory, queue_size=1 + ) + right_arm_cmd_msg = init_cmd_msg( + right_arm_cmd_msg, + [ + "arm_right_1_joint", + "arm_right_2_joint", + "arm_right_3_joint", + "arm_right_4_joint", + "arm_right_5_joint", + "arm_right_6_joint", + "arm_right_7_joint", + ], + ) + + torso_cmd_pub = rospy.Publisher( + "/torso_controller/command", JointTrajectory, queue_size=1 + ) torso_cmd_msg = init_cmd_msg(torso_cmd_msg, ["torso_lift_joint"]) - mobile_base_cmd_pub = rospy.Publisher('/mobile_base_controller/cmd_vel', Twist, queue_size=1) + mobile_base_cmd_pub = rospy.Publisher( + "/mobile_base_controller/cmd_vel", Twist, queue_size=1 + ) + + # Get (possible) enable/disable parameter and set up service to change it + if rospy.has_param("~send_commands"): + send_commands = rospy.get_param("~send_commands") + rospy.Service("send_commands", SetBool, set_send_commands) + rospy.loginfo(f"send_commands: {send_commands}") - if rospy.has_param('~send_commands'): - send_commands=rospy.get_param('~send_commands') - rospy.Service('send_commands', SetBool, set_send_commands) - print(f"send_commands: {send_commands}") + # Get (possible) 'time_from_start' parameter and set up subscriber to change it + if rospy.has_param("~time_from_start"): + time_from_start = rospy.get_param("~time_from_start") + rospy.loginfo(f"time_from_start: {time_from_start}") + rospy.Subscriber("ros_control_bridge/time_from_start", Float32, time_from_start_cb) + # Set up subscriber to CartesI/O solution topic rospy.Subscriber("cartesian/solution", JointState, io_callback) - rate = rospy.Rate(rate_) - while not rospy.is_shutdown(): - rate.sleep() + rospy.spin() diff --git a/stack/tiago_dual.stack b/stack/tiago_dual.stack index f643ebe..6b85d9c 100644 --- a/stack/tiago_dual.stack +++ b/stack/tiago_dual.stack @@ -71,6 +71,21 @@ Base2D: JointLimits: type: "JointLimits" + limits: + arm_right_1_joint: [-1.1080972450961724, 1.5007963267948965] + arm_right_2_joint: [-1.1080972450961724, 1.5007963267948965] + arm_right_3_joint: [-0.7153981633974482, 3.8569908169872416] + arm_right_4_joint: [-0.32269908169872413, 2.286194490192345] + arm_right_5_joint: [-2.0743951023931952, 2.0743951023931952] + arm_right_6_joint: [-1.3937166941154069, 1.3937166941154069] + arm_right_7_joint: [-2.0743951023931952, 2.0743951023931952] + arm_left_1_joint: [-1.1080972450961724, 1.5007963267948965] + arm_left_2_joint: [-1.1080972450961724, 1.5007963267948965] + arm_left_3_joint: [-0.7153981633974482, 3.8569908169872416] + arm_left_4_joint: [-0.32269908169872413, 2.286194490192345] + arm_left_5_joint: [-2.0743951023931952, 2.0743951023931952] + arm_left_6_joint: [-1.3937166941154069, 1.3937166941154069] + arm_left_7_joint: [-2.0743951023931952, 2.0743951023931952] VelocityLimits: type: "VelocityLimits" @@ -146,4 +161,3 @@ Collision: - [torso_fixed_column_link, gripper_left_right_finger_link] - From ab514d7824cd9bdcaf66d036f845591f80403731 Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Tue, 24 Sep 2024 15:40:12 +0200 Subject: [PATCH 15/56] Adjysted right camera tf --- CMakeLists.txt | 201 +---------------------------------- package.xml | 51 +-------- robots/tiago_dual.urdf.xacro | 2 +- 3 files changed, 3 insertions(+), 251 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c4fb548..d610dcf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,204 +1,5 @@ cmake_minimum_required(VERSION 3.0.2) project(tiago_dual_cartesio_config) -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) +find_package(catkin REQUIRED) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - cartesian_interface -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES franka_cartesio_config -# CATKIN_DEPENDS cartesian_interface franka_description -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/franka_cartesio_config.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/franka_cartesio_config_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_franka_cartesio_config.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/package.xml b/package.xml index 5307816..ae37f27 100644 --- a/package.xml +++ b/package.xml @@ -4,59 +4,10 @@ 0.0.0 The tiago_dual_cartesio_config package - - - enrico - - - - TODO + catkin - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - cartesian_interface - cartesian_interface - cartesian_interface - - - - - - - diff --git a/robots/tiago_dual.urdf.xacro b/robots/tiago_dual.urdf.xacro index 364bcc4..23d46d4 100644 --- a/robots/tiago_dual.urdf.xacro +++ b/robots/tiago_dual.urdf.xacro @@ -12,7 +12,7 @@ - + From 72eecb8dc57b565e78f1578910c7eda05daba12a Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Tue, 24 Sep 2024 17:35:44 +0200 Subject: [PATCH 16/56] Update camera_right_frame --- capsules/urdf/tiago_dual_capsules.rviz | 2 +- robots/tiago_dual.urdf.xacro | 4 +--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/capsules/urdf/tiago_dual_capsules.rviz b/capsules/urdf/tiago_dual_capsules.rviz index e282fb5..ab9a7c7 100644 --- a/capsules/urdf/tiago_dual_capsules.rviz +++ b/capsules/urdf/tiago_dual_capsules.rviz @@ -2344,7 +2344,7 @@ - + diff --git a/robots/tiago_dual.urdf.xacro b/robots/tiago_dual.urdf.xacro index 23d46d4..2d0802f 100644 --- a/robots/tiago_dual.urdf.xacro +++ b/robots/tiago_dual.urdf.xacro @@ -12,11 +12,9 @@ - + - - From 3e7435d52e60b77ab507679e8732e65f28fadcb2 Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Wed, 25 Sep 2024 09:35:39 +0200 Subject: [PATCH 17/56] Adding static_tf odom=ci/world --- launch/cartesio.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/cartesio.launch b/launch/cartesio.launch index 816130b..fbcd6f9 100644 --- a/launch/cartesio.launch +++ b/launch/cartesio.launch @@ -57,7 +57,7 @@ - + From b2386a0e5c90c10bd3d960d38c390eb735041ea1 Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Wed, 25 Sep 2024 13:33:38 +0200 Subject: [PATCH 18/56] Removed Gaze and Omni4X tasks from stack --- stack/tiago_dual.stack | 18 +----------------- 1 file changed, 1 insertion(+), 17 deletions(-) diff --git a/stack/tiago_dual.stack b/stack/tiago_dual.stack index 6b85d9c..3467373 100644 --- a/stack/tiago_dual.stack +++ b/stack/tiago_dual.stack @@ -3,21 +3,11 @@ solver_options: back_end: "qpoases" stack: - - ["Gaze"] - ["Lee", "Ree", "Base"] - ["Postural"] -constraints: ["JointLimits", "VelocityLimits", "Base2D", "Omni4X", "Collision"] - - -Omni4X: - type: "OmniWheels4X" - l1: 0.223 - l2: 0.244 - wheel_radius: 0.08 - base_link: "base_link" - joint_wheels_name: ["wheel_front_left_joint", "wheel_front_right_joint", "wheel_rear_left_joint", "wheel_rear_right_joint"] +constraints: ["JointLimits", "VelocityLimits", "Base2D", "Collision"] Lee: type: "Cartesian" @@ -31,12 +21,6 @@ Ree: base_link: "base_link" lambda: 0.1 -Gaze: - type: "Gaze" - distal_link: "head_front_camera_link" - base_link: "base_link" - lambda: 1. - Postural: type: "Postural" lambda: 0.1 From 796172e26657fb3da6fbc2f45413157db336eb52 Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Thu, 26 Sep 2024 12:46:33 +0200 Subject: [PATCH 19/56] Update rviz settings --- launch/viz.rviz | 260 +++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 246 insertions(+), 14 deletions(-) diff --git a/launch/viz.rviz b/launch/viz.rviz index 666c76b..6514552 100644 --- a/launch/viz.rviz +++ b/launch/viz.rviz @@ -7,9 +7,11 @@ Panels: - /Global Options1 - /Status1 - /InteractiveMarkers1 - - /Marker1/Status1 - Splitter Ratio: 0.5 - Tree Height: 719 + - /TF1 + - /TF1/Frames1 + - /TF1/Tree1/odom1 + Splitter Ratio: 0.6153846383094788 + Tree Height: 725 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -179,6 +181,10 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + camera_right_frame: + Alpha: 1 + Show Axes: false + Show Trail: false gripper_left_grasping_frame: Alpha: 1 Show Axes: false @@ -365,17 +371,243 @@ Visualization Manager: Show Visual Aids: false Update Topic: /gripper_right_grasping_frame_cartesian_marker_server/update Value: true - - Class: rviz/Marker + - Class: rviz/TF Enabled: true - Marker Topic: /cartesian/collision_avoidance/collision_distances - Name: Marker - Namespaces: - "": true - Queue Size: 100 + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: false + ci/arm_left_1_link: + Value: false + ci/arm_left_2_link: + Value: false + ci/arm_left_3_link: + Value: false + ci/arm_left_4_link: + Value: false + ci/arm_left_5_link: + Value: false + ci/arm_left_6_link: + Value: false + ci/arm_left_7_link: + Value: false + ci/arm_left_tool_link: + Value: false + ci/arm_right_1_link: + Value: false + ci/arm_right_2_link: + Value: false + ci/arm_right_3_link: + Value: false + ci/arm_right_4_link: + Value: false + ci/arm_right_5_link: + Value: false + ci/arm_right_6_link: + Value: false + ci/arm_right_7_link: + Value: false + ci/arm_right_tool_link: + Value: false + ci/base_antenna_left_link: + Value: false + ci/base_antenna_right_link: + Value: false + ci/base_dock_link: + Value: false + ci/base_footprint: + Value: false + ci/base_front_laser_link: + Value: false + ci/base_imu_link: + Value: false + ci/base_link: + Value: true + ci/base_rear_laser_link: + Value: false + ci/camera_right_frame: + Value: false + ci/com: + Value: false + ci/gripper_left_grasping_frame: + Value: true + ci/gripper_left_left_finger_link: + Value: false + ci/gripper_left_link: + Value: false + ci/gripper_left_right_finger_link: + Value: false + ci/gripper_left_tool_link: + Value: false + ci/gripper_right_grasping_frame: + Value: true + ci/gripper_right_left_finger_link: + Value: false + ci/gripper_right_link: + Value: false + ci/gripper_right_right_finger_link: + Value: false + ci/gripper_right_tool_link: + Value: false + ci/head_1_link: + Value: false + ci/head_2_link: + Value: false + ci/head_front_camera_depth_frame: + Value: false + ci/head_front_camera_depth_optical_frame: + Value: false + ci/head_front_camera_link: + Value: false + ci/head_front_camera_optical_frame: + Value: false + ci/head_front_camera_orbbec_aux_joint_frame: + Value: false + ci/head_front_camera_rgb_frame: + Value: false + ci/head_front_camera_rgb_optical_frame: + Value: false + ci/rgbd_laser_link: + Value: false + ci/suspension_front_left_link: + Value: false + ci/suspension_front_right_link: + Value: false + ci/suspension_rear_left_link: + Value: false + ci/suspension_rear_right_link: + Value: false + ci/torso_fixed_column_link: + Value: false + ci/torso_fixed_link: + Value: false + ci/torso_lift_link: + Value: false + ci/virtual_base_laser_link: + Value: false + ci/wheel_front_left_link: + Value: false + ci/wheel_front_right_link: + Value: false + ci/wheel_rear_left_link: + Value: false + ci/wheel_rear_right_link: + Value: false + ci/world: + Value: false + ci/wrist_left_ft_link: + Value: false + ci/wrist_left_ft_tool_link: + Value: false + ci/wrist_right_ft_link: + Value: false + ci/wrist_right_ft_tool_link: + Value: false + odom: + Value: false + Marker Alpha: 1 + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + odom: + ci/world: + ci/base_footprint: + ci/base_link: + ci/base_antenna_left_link: + {} + ci/base_antenna_right_link: + {} + ci/base_dock_link: + {} + ci/base_front_laser_link: + {} + ci/base_imu_link: + {} + ci/base_rear_laser_link: + {} + ci/suspension_front_left_link: + ci/wheel_front_left_link: + {} + ci/suspension_front_right_link: + ci/wheel_front_right_link: + {} + ci/suspension_rear_left_link: + ci/wheel_rear_left_link: + {} + ci/suspension_rear_right_link: + ci/wheel_rear_right_link: + {} + ci/torso_fixed_column_link: + {} + ci/torso_fixed_link: + ci/torso_lift_link: + ci/arm_left_1_link: + ci/arm_left_2_link: + ci/arm_left_3_link: + ci/arm_left_4_link: + ci/arm_left_5_link: + ci/arm_left_6_link: + ci/arm_left_7_link: + ci/arm_left_tool_link: + ci/wrist_left_ft_link: + ci/wrist_left_ft_tool_link: + ci/gripper_left_link: + ci/gripper_left_grasping_frame: + {} + ci/gripper_left_left_finger_link: + {} + ci/gripper_left_right_finger_link: + {} + ci/gripper_left_tool_link: + {} + ci/arm_right_1_link: + ci/arm_right_2_link: + ci/arm_right_3_link: + ci/arm_right_4_link: + ci/arm_right_5_link: + ci/arm_right_6_link: + ci/arm_right_7_link: + ci/arm_right_tool_link: + ci/wrist_right_ft_link: + ci/wrist_right_ft_tool_link: + ci/camera_right_frame: + {} + ci/gripper_right_link: + ci/gripper_right_grasping_frame: + {} + ci/gripper_right_left_finger_link: + {} + ci/gripper_right_right_finger_link: + {} + ci/gripper_right_tool_link: + {} + ci/head_1_link: + ci/head_2_link: + ci/head_front_camera_link: + ci/head_front_camera_optical_frame: + {} + ci/head_front_camera_orbbec_aux_joint_frame: + ci/head_front_camera_rgb_frame: + ci/head_front_camera_depth_frame: + ci/head_front_camera_depth_optical_frame: + {} + ci/head_front_camera_rgb_optical_frame: + {} + ci/virtual_base_laser_link: + {} + ci/rgbd_laser_link: + {} + ci/com: + {} + Update Interval: 0 Value: true Enabled: true Global Options: - Background Color: 255; 255; 255 + Background Color: 0; 0; 0 Default Light: true Fixed Frame: ci/world Frame Rate: 30 @@ -401,7 +633,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.7909255027770996 + Distance: 2.7537271976470947 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -417,9 +649,9 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.3553982377052307 + Pitch: 0.2403983473777771 Target Frame: - Yaw: 0.02539195865392685 + Yaw: 0.3503918945789337 Saved: ~ Window Geometry: Displays: @@ -427,7 +659,7 @@ Window Geometry: Height: 1016 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a0000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000005c80000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001950000035efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000030700fffffffb0000000800540069006d006501000000000000045000000000000000000000059d0000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: From 4be4e6097800dd3a37aa58bfe87fa28a1b4b66cf Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Thu, 26 Sep 2024 17:51:08 +0200 Subject: [PATCH 20/56] Added camera_left_frame --- capsules/urdf/tiago_dual_capsules.rviz | 17 ++++++++++++----- robots/tiago_dual.urdf.xacro | 7 +++++++ 2 files changed, 19 insertions(+), 5 deletions(-) diff --git a/capsules/urdf/tiago_dual_capsules.rviz b/capsules/urdf/tiago_dual_capsules.rviz index ab9a7c7..07c4abc 100644 --- a/capsules/urdf/tiago_dual_capsules.rviz +++ b/capsules/urdf/tiago_dual_capsules.rviz @@ -2343,9 +2343,16 @@ - - - - - + + + + + + + + + + + + diff --git a/robots/tiago_dual.urdf.xacro b/robots/tiago_dual.urdf.xacro index 2d0802f..86d379d 100644 --- a/robots/tiago_dual.urdf.xacro +++ b/robots/tiago_dual.urdf.xacro @@ -17,4 +17,11 @@ + + + + + + + From baf3906b8a66601360b5ffabcacb2d3d003b9ead Mon Sep 17 00:00:00 2001 From: Clemente Donoso Date: Thu, 26 Sep 2024 18:05:30 +0200 Subject: [PATCH 21/56] - Updated launch files to support teleoperation with both left and right arms - Ensured proper communication and control for dual-arm operation - Tested and verified functionality for both arms simultaneously --- launch/teleop.launch | 36 +++++++++++++++++++++++++++++++----- 1 file changed, 31 insertions(+), 5 deletions(-) diff --git a/launch/teleop.launch b/launch/teleop.launch index 38b5a6b..c2094d1 100644 --- a/launch/teleop.launch +++ b/launch/teleop.launch @@ -1,10 +1,36 @@ - + + + + + + + + + + + + + + + + + + $(arg controller_gripper_joints) + + + + + + + + - + + @@ -12,12 +38,12 @@ - + $(arg controller_gripper_joints) - Ù + - + \ No newline at end of file From 30854a92f632434916c8f6568a0be1ca9da47e84 Mon Sep 17 00:00:00 2001 From: Clemente Donoso Date: Fri, 27 Sep 2024 15:29:22 +0200 Subject: [PATCH 22/56] Change arg names for each arm and gripper --- launch/teleop.launch | 56 ++++++++++++++++++++++---------------------- 1 file changed, 28 insertions(+), 28 deletions(-) diff --git a/launch/teleop.launch b/launch/teleop.launch index c2094d1..653b340 100644 --- a/launch/teleop.launch +++ b/launch/teleop.launch @@ -1,49 +1,49 @@ - + - + - - - - - - + + + + + + - - $(arg controller_gripper_joints) - - - - + + $(arg left_controller_gripper_joints) + + + + - + - + - - - - - - + + + + + + - - $(arg controller_gripper_joints) - - - - + + $(arg right_controller_gripper_joints) + + + + \ No newline at end of file From f8809a698b6ea77913582e96ca4cacb2fe0696b7 Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Tue, 1 Oct 2024 10:34:52 +0200 Subject: [PATCH 23/56] Removed left joint 7 from tiago_dual.stack --- stack/tiago_dual.stack | 3 +++ 1 file changed, 3 insertions(+) diff --git a/stack/tiago_dual.stack b/stack/tiago_dual.stack index 3467373..7f9c11f 100644 --- a/stack/tiago_dual.stack +++ b/stack/tiago_dual.stack @@ -14,6 +14,8 @@ Lee: distal_link: "gripper_left_grasping_frame" base_link: "base_link" lambda: 0.1 + disabled_joints: + - "arm_left_7_joint" Ree: type: "Cartesian" @@ -36,6 +38,7 @@ Postural: wheel_front_right_joint: 0.0 wheel_rear_left_joint: 0.0 wheel_rear_right_joint: 0.0 + arm_left_7_joint: 0.0 Base: type: "Cartesian" From eebf06ad4096c85fa3ed4a63e8c3d4180c8c6ae4 Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Tue, 1 Oct 2024 17:07:53 +0200 Subject: [PATCH 24/56] Added log to ros_control_bridge/send_commands --- python/ros_control_bridge.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/python/ros_control_bridge.py b/python/ros_control_bridge.py index cce8366..df12b42 100644 --- a/python/ros_control_bridge.py +++ b/python/ros_control_bridge.py @@ -113,7 +113,8 @@ def set_send_commands(req: SetBool): send_commands = req.data response = SetBoolResponse() response.success = True - response.message = "send_response set to " + str(req.data) + response.message = "send_commands set to " + str(req.data) + rospy.loginfo(f"Setting send_commands={req.data}") return response @@ -187,7 +188,7 @@ def time_from_start_cb(msg: Float32): # Get (possible) enable/disable parameter and set up service to change it if rospy.has_param("~send_commands"): send_commands = rospy.get_param("~send_commands") - rospy.Service("send_commands", SetBool, set_send_commands) + rospy.Service("ros_control_bridge/send_commands", SetBool, set_send_commands) rospy.loginfo(f"send_commands: {send_commands}") # Get (possible) 'time_from_start' parameter and set up subscriber to change it From 715947976f56d4a8e5c5e93f4174a593f82ec386 Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Fri, 4 Oct 2024 10:37:33 +0200 Subject: [PATCH 25/56] Added head camera frame --- capsules/urdf/tiago_dual_capsules.rviz | 11 ++++++++++- robots/tiago_dual.urdf.xacro | 8 ++++++++ 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/capsules/urdf/tiago_dual_capsules.rviz b/capsules/urdf/tiago_dual_capsules.rviz index 07c4abc..9557200 100644 --- a/capsules/urdf/tiago_dual_capsules.rviz +++ b/capsules/urdf/tiago_dual_capsules.rviz @@ -2348,11 +2348,20 @@ - + + + + + + + + + + diff --git a/robots/tiago_dual.urdf.xacro b/robots/tiago_dual.urdf.xacro index 86d379d..d3555b8 100644 --- a/robots/tiago_dual.urdf.xacro +++ b/robots/tiago_dual.urdf.xacro @@ -24,4 +24,12 @@ + + + + + + + + From 99858a19cd5bb4e78fa1819342ee3a25ef5a2b49 Mon Sep 17 00:00:00 2001 From: Clemente Donoso Date: Fri, 4 Oct 2024 18:35:21 +0200 Subject: [PATCH 26/56] Add functionality to klill the node with the streamdeck --- python/ros_control_bridge.py | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/python/ros_control_bridge.py b/python/ros_control_bridge.py index df12b42..93450de 100644 --- a/python/ros_control_bridge.py +++ b/python/ros_control_bridge.py @@ -1,10 +1,11 @@ #!/usr/bin/env python3 import rospy +import os from sensor_msgs.msg import JointState from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from geometry_msgs.msg import Twist -from std_msgs.msg import Float32 +from std_msgs.msg import Float32, Bool from std_srvs.srv import SetBool, SetBoolResponse head_cmd_pub = None @@ -24,6 +25,8 @@ time_from_start = 0.2 +exit_ = False + send_commands = True SEND_VELOCITY = False @@ -129,6 +132,10 @@ def time_from_start_cb(msg: Float32): "Trying to set a 'time_from_start' too low. It must be >= 0.1secs" ) +def control_initiator_cb(msg): + global exit_ + if not msg.data: + exit_ = True if __name__ == "__main__": rospy.init_node("ros_control_bridge", anonymous=True) @@ -200,4 +207,11 @@ def time_from_start_cb(msg: Float32): # Set up subscriber to CartesI/O solution topic rospy.Subscriber("cartesian/solution", JointState, io_callback) - rospy.spin() + # Set up a subscriber to kill the node + rospy.Subscriber('/streamdeck/ros_control_bridge_initiator', Bool, control_initiator_cb) + + while not exit_ or not rospy.is_shutdown(): + rospy.spin() + + rospy.loginfo("Exiting ros_control_bridge") + os._exit(0) From 9c1105900d148ed12063cc41cd6ee2d1da2fba2d Mon Sep 17 00:00:00 2001 From: Clemente Donoso Krauss <49209768+CDonosoK@users.noreply.github.com> Date: Mon, 7 Oct 2024 12:51:04 +0200 Subject: [PATCH 27/56] Update ros_control_bridge.py Now you can kill the node using the streamdeck and also using ctrl+c --- python/ros_control_bridge.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/python/ros_control_bridge.py b/python/ros_control_bridge.py index 93450de..8c55837 100644 --- a/python/ros_control_bridge.py +++ b/python/ros_control_bridge.py @@ -210,8 +210,14 @@ def control_initiator_cb(msg): # Set up a subscriber to kill the node rospy.Subscriber('/streamdeck/ros_control_bridge_initiator', Bool, control_initiator_cb) - while not exit_ or not rospy.is_shutdown(): + while not rospy.is_shutdown(): + if exit_: + break rospy.spin() rospy.loginfo("Exiting ros_control_bridge") os._exit(0) + + + rospy.loginfo("Exiting ros_control_bridge") + os._exit(0) From 45bf6af186200296899344c397c1445f64ffcf28 Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Mon, 7 Oct 2024 14:43:23 +0200 Subject: [PATCH 28/56] Added demo_recorder (and minor formatting) --- launch/demo_recorder.launch | 6 + python/demo_recorder.py | 251 ++++++++++++++++++++++++++++++++ python/ros_control_bridge.py | 7 +- python/teleop_bridge.py | 37 +++-- python/teleop_gripper_bridge.py | 59 ++++---- 5 files changed, 322 insertions(+), 38 deletions(-) create mode 100644 launch/demo_recorder.launch create mode 100755 python/demo_recorder.py diff --git a/launch/demo_recorder.launch b/launch/demo_recorder.launch new file mode 100644 index 0000000..c9cd31f --- /dev/null +++ b/launch/demo_recorder.launch @@ -0,0 +1,6 @@ + + + "/home" + [0, 1] + + diff --git a/python/demo_recorder.py b/python/demo_recorder.py new file mode 100755 index 0000000..be488a0 --- /dev/null +++ b/python/demo_recorder.py @@ -0,0 +1,251 @@ +#!/usr/bin/env python3 + +import numpy as np +import threading +import os + +import rospy +import tf2_ros + +from sensor_msgs.msg import JointState +from trajectory_msgs.msg import JointTrajectory +from geometry_msgs.msg import PoseStamped + + +class DemoRecorder: + def __init__(self): + rospy.init_node("demo_recorder") + + # Load params + if rospy.has_param("~save_folder"): + self.save_folder = rospy.get_param("~save_folder") + else: + self.save_folder = os.environ["HOME"] + + if rospy.has_param("~tags"): + tags = rospy.get_param("~tags") + else: + tags = [] + + # Set dump file on node shutdown + rospy.on_shutdown(self.dump_data) + + # Set up tf2 listener + self.tf_buffer = tf2_ros.Buffer() + tf2_ros.TransformListener(self.tf_buffer) + + # Wait for tf listener + rospy.sleep(1) + + # Init ee pose recorder + self.actual_left_pose_data = np.zeros((1, 8)) + self.actual_right_pose_data = np.zeros((1, 8)) + self.target_left_pose_data = np.zeros((1, 8)) + self.target_right_pose_data = np.zeros((1, 8)) + + # Init joint state recorder + self.joints_data = np.zeros((1, 28)) + rospy.Subscriber("/joint_states", JointState, self.cb_joint_state) + + # Init left/right gripper recorder + self.left_gripper_data = np.zeros((1, 3)) + self.right_gripper_data = np.zeros((1, 3)) + rospy.Subscriber( + "/gripper_left_controller/command", JointTrajectory, self.cb_left_gripper + ) + rospy.Subscriber( + "/gripper_right_controller/command", JointTrajectory, self.cb_right_gripper + ) + + # Init recorder(s) for april tags + self.tags_data = {} + for tag in tags: + self.tags_data[int(tag)] = np.zeros((1, 8)) + rospy.Subscriber(f"/pose_tag_{tag}", PoseStamped, self.cb_tag, (int(tag))) + + def cb_joint_state(self, msg: JointState): + """Store joints state data (format: nPoints x [j1, ..., j27, time_stamp])""" + new_joints = np.zeros((1, 28)) + for i in range(27): + new_joints[0, i] = msg.position[i] + new_joints[0, -1] = msg.header.stamp.secs + 10**-9 * msg.header.stamp.nsecs + self.joints_data = np.append(self.joints_data, new_joints, 0) + + def cb_left_gripper(self, msg: JointTrajectory): + """Store left gripper data (format: nPoints x [joint_left, joint_right, time_stamp])""" + new_left_gripper = np.zeros((1, 3)) + new_left_gripper[0, 0] = msg.points[0].positions[ + msg.joint_names.index("gripper_left_left_finger_joint") + ] + new_left_gripper[0, 1] = msg.points[0].positions[ + msg.joint_names.index("gripper_left_right_finger_joint") + ] + new_left_gripper[0, 2] = msg.header.stamp.secs + 10**-9 * msg.header.stamp.nsecs + self.left_gripper_data = np.append(self.left_gripper_data, new_left_gripper, 0) + + def cb_right_gripper(self, msg: JointTrajectory): + """Store right gripper data (format: nPoints x [joint_left, joint_right, time_stamp])""" + new_right_gripper = np.zeros((1, 3)) + new_right_gripper[0, 0] = msg.points[0].positions[ + msg.joint_names.index("gripper_right_left_finger_joint") + ] + new_right_gripper[0, 1] = msg.points[0].positions[ + msg.joint_names.index("gripper_right_right_finger_joint") + ] + new_right_gripper[0, 2] = ( + msg.header.stamp.secs + 10**-9 * msg.header.stamp.nsecs + ) + self.right_gripper_data = np.append( + self.right_gripper_data, new_right_gripper, 0 + ) + + def cb_tag(self, msg: PoseStamped, args: int): + """Store tags data (format: nTagsx [nPoints x [x, y, z, qx, qy, qz, qw, time_stamp]])""" + tag_id = int(args[0]) + new_tag = np.zeros((1, 8)) + new_tag[0, 0] = msg.pose.position.x + new_tag[0, 1] = msg.pose.position.y + new_tag[0, 2] = msg.pose.position.z + new_tag[0, 3] = msg.pose.position.x + new_tag[0, 4] = msg.pose.position.y + new_tag[0, 5] = msg.pose.position.z + new_tag[0, 6] = msg.pose.position.w + new_tag[0, 7] = msg.header.stamp.secs + 10**-9 * msg.header.stamp.nsecs + self.tags_data[tag_id] = np.append(self.tags_data[tag_id], new_tag, 0) + + def record(self): + """Query tf tree and store target and actual ee poses (format: nPoints x [x, y, z, qx, qy, qz, qw, time_stamp])""" + rate = rospy.Rate(100) + while not rospy.is_shutdown(): + now = rospy.get_rostime() + + try: + # base-to-left-ee (actual) + t = self.tf_buffer.lookup_transform( + "base_link", "gripper_left_grasping_frame", now + ) + new_actual_left_pose = np.zeros((1, 8)) + new_actual_left_pose[0, 1] = t.transform.translation.x + new_actual_left_pose[0, 2] = t.transform.translation.y + new_actual_left_pose[0, 3] = t.transform.translation.z + new_actual_left_pose[0, 4] = t.transform.rotation.x + new_actual_left_pose[0, 5] = t.transform.rotation.y + new_actual_left_pose[0, 6] = t.transform.rotation.z + new_actual_left_pose[0, 7] = t.transform.rotation.w + new_actual_left_pose[0, 7] = now.to_sec() + self.actual_left_pose_data = np.append( + self.actual_left_pose_data, new_actual_left_pose, 0 + ) + except Exception as err: + rospy.logwarn(err) + + try: + # base-to-right-ee (actual) + t = self.tf_buffer.lookup_transform( + "base_link", "gripper_right_grasping_frame", now + ) + new_actual_right_pose = np.zeros((1, 8)) + new_actual_right_pose[0, 1] = t.transform.translation.x + new_actual_right_pose[0, 2] = t.transform.translation.y + new_actual_right_pose[0, 3] = t.transform.translation.z + new_actual_right_pose[0, 4] = t.transform.rotation.x + new_actual_right_pose[0, 5] = t.transform.rotation.y + new_actual_right_pose[0, 6] = t.transform.rotation.z + new_actual_right_pose[0, 7] = t.transform.rotation.w + new_actual_right_pose[0, 7] = now.to_sec() + self.actual_right_pose_data = np.append( + self.actual_right_pose_data, new_actual_right_pose, 0 + ) + except Exception as err: + rospy.logwarn(err) + + try: + # base-to-left-ee (target) + t = self.tf_buffer.lookup_transform( + "ci/base_link", "ci/gripper_left_grasping_frame", now + ) + new_target_left_pose = np.zeros((1, 8)) + new_target_left_pose[0, 1] = t.transform.translation.x + new_target_left_pose[0, 2] = t.transform.translation.y + new_target_left_pose[0, 3] = t.transform.translation.z + new_target_left_pose[0, 4] = t.transform.rotation.x + new_target_left_pose[0, 5] = t.transform.rotation.y + new_target_left_pose[0, 6] = t.transform.rotation.z + new_target_left_pose[0, 7] = t.transform.rotation.w + new_target_left_pose[0, 7] = now.to_sec() + self.target_left_pose_data = np.append( + self.target_left_pose_data, new_target_left_pose, 0 + ) + except Exception as err: + rospy.logwarn(err) + + try: + # base-to-right-ee (target) + t = self.tf_buffer.lookup_transform( + "ci/base_link", "ci/gripper_right_grasping_frame", now + ) + new_target_right_pose = np.zeros((1, 8)) + new_target_right_pose[0, 1] = t.transform.translation.x + new_target_right_pose[0, 2] = t.transform.translation.y + new_target_right_pose[0, 3] = t.transform.translation.z + new_target_right_pose[0, 4] = t.transform.rotation.x + new_target_right_pose[0, 5] = t.transform.rotation.y + new_target_right_pose[0, 6] = t.transform.rotation.z + new_target_right_pose[0, 7] = t.transform.rotation.w + new_target_right_pose[0, 7] = now.to_sec() + self.target_right_pose_data = np.append( + self.target_right_pose_data, new_target_right_pose, 0 + ) + except Exception as err: + rospy.logwarn(err) + + rate.sleep() + + def dump_data(self): + rospy.logwarn(f"Dumping recorded data into '{self.save_folder}'") + np.savetxt( + f"{self.save_folder}/actual_left_pose_data.csv", + self.actual_left_pose_data, + delimiter=",", + ) + np.savetxt( + f"{self.save_folder}/actual_right_pose_data.csv", + self.actual_right_pose_data, + delimiter=",", + ) + np.savetxt( + f"{self.save_folder}/target_left_pose_data.csv", + self.target_left_pose_data, + delimiter=",", + ) + np.savetxt( + f"{self.save_folder}/target_right_pose_data.csv", + self.target_right_pose_data, + delimiter=",", + ) + np.savetxt( + f"{self.save_folder}/left_gripper_data.csv", + self.left_gripper_data, + delimiter=",", + ) + np.savetxt( + f"{self.save_folder}/right_gripper_data.csv", + self.right_gripper_data, + delimiter=",", + ) + + for tag in self.tags_data: + np.savetxt( + f"{self.save_folder}/pose_tag_{tag}.csv", + self.tags_data[tag], + delimiter=",", + ) + + +if __name__ == "__main__": + dr = DemoRecorder() + dr.record() + dr_thread = threading.Thread(target=dr.record) + dr_thread.start() + + rospy.spin() diff --git a/python/ros_control_bridge.py b/python/ros_control_bridge.py index 8c55837..c8f80aa 100644 --- a/python/ros_control_bridge.py +++ b/python/ros_control_bridge.py @@ -132,11 +132,13 @@ def time_from_start_cb(msg: Float32): "Trying to set a 'time_from_start' too low. It must be >= 0.1secs" ) + def control_initiator_cb(msg): global exit_ if not msg.data: exit_ = True + if __name__ == "__main__": rospy.init_node("ros_control_bridge", anonymous=True) rospy.loginfo(f"SEND_VELOCITY: {SEND_VELOCITY}") @@ -208,7 +210,9 @@ def control_initiator_cb(msg): rospy.Subscriber("cartesian/solution", JointState, io_callback) # Set up a subscriber to kill the node - rospy.Subscriber('/streamdeck/ros_control_bridge_initiator', Bool, control_initiator_cb) + rospy.Subscriber( + "/streamdeck/ros_control_bridge_initiator", Bool, control_initiator_cb + ) while not rospy.is_shutdown(): if exit_: @@ -218,6 +222,5 @@ def control_initiator_cb(msg): rospy.loginfo("Exiting ros_control_bridge") os._exit(0) - rospy.loginfo("Exiting ros_control_bridge") os._exit(0) diff --git a/python/teleop_bridge.py b/python/teleop_bridge.py index 5822f22..031fe07 100755 --- a/python/teleop_bridge.py +++ b/python/teleop_bridge.py @@ -12,6 +12,7 @@ initialized = False controlled_frame = str() + def callback(data: PoseStamped): global initial_ref, initialized, controlled_frame, pose_initial, ci, pose_initial @@ -23,17 +24,35 @@ def callback(data: PoseStamped): rospy.loginfo("correclty initialized") else: r_init = R.from_quat( - [initial_ref.pose.orientation.x, initial_ref.pose.orientation.y, initial_ref.pose.orientation.z, - initial_ref.pose.orientation.w]) + [ + initial_ref.pose.orientation.x, + initial_ref.pose.orientation.y, + initial_ref.pose.orientation.z, + initial_ref.pose.orientation.w, + ] + ) data_m_init = Affine3() data_m_init.translation = np.array( - [initial_ref.pose.position.x, initial_ref.pose.position.y, initial_ref.pose.position.z]) + [ + initial_ref.pose.position.x, + initial_ref.pose.position.y, + initial_ref.pose.position.z, + ] + ) data_m_init.linear = r_init.as_matrix() r = R.from_quat( - [data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w]) + [ + data.pose.orientation.x, + data.pose.orientation.y, + data.pose.orientation.z, + data.pose.orientation.w, + ] + ) data_m = Affine3() - data_m.translation = np.array([data.pose.position.x, data.pose.position.y, data.pose.position.z]) + data_m.translation = np.array( + [data.pose.position.x, data.pose.position.y, data.pose.position.z] + ) data_m.linear = r.as_matrix() ref_m = pose_initial * data_m_init.inverse() * data_m @@ -45,11 +64,11 @@ def callback(data: PoseStamped): ci.setPoseReference(controlled_frame, pose_ref) -if __name__ == '__main__': - rospy.init_node('teleop_bridge', anonymous=True) +if __name__ == "__main__": + rospy.init_node("teleop_bridge", anonymous=True) - if rospy.has_param('~controlled_frame'): - controlled_frame = rospy.get_param('~controlled_frame') + if rospy.has_param("~controlled_frame"): + controlled_frame = rospy.get_param("~controlled_frame") else: rospy.logerr("controlled_frame private param is mandatory! Exiting.") exit() diff --git a/python/teleop_gripper_bridge.py b/python/teleop_gripper_bridge.py index a35a224..781fb1a 100755 --- a/python/teleop_gripper_bridge.py +++ b/python/teleop_gripper_bridge.py @@ -10,11 +10,12 @@ GRIPPER_MAX = 0.044 GRIPPER_MIN = 0.010 -TELEOP_MAX = 0. -TELEOP_MIN = 1. +TELEOP_MAX = 0.0 +TELEOP_MIN = 1.0 + +rate_ = 50.0 +time_duration = 0.2 -rate_ = 50. -time_duration = 0.2 def print_info(): rospy.loginfo(f" GRIPPER_MAX: {GRIPPER_MAX}") @@ -22,55 +23,63 @@ def print_info(): rospy.loginfo(f" TELEOP_MAX: {TELEOP_MAX}") rospy.loginfo(f" TELEOP_MIN: {TELEOP_MIN}") + def io_callback(data: Float32): - a = (GRIPPER_MAX - GRIPPER_MIN)/(TELEOP_MAX-TELEOP_MIN) + a = (GRIPPER_MAX - GRIPPER_MIN) / (TELEOP_MAX - TELEOP_MIN) d = data.data gripper = a * d * (d - TELEOP_MAX) + GRIPPER_MAX for joint_name in gripper_cmd_msg.joint_names: - gripper_cmd_msg.points[0].positions[gripper_cmd_msg.joint_names.index(joint_name)] = gripper + gripper_cmd_msg.points[0].positions[ + gripper_cmd_msg.joint_names.index(joint_name) + ] = gripper gripper_cmd_msg.points[0].time_from_start = rospy.Duration(time_duration) + gripper_cmd_msg.header.stamp = rospy.get_rostime() gripper_cmd_pub.publish(gripper_cmd_msg) + def init_cmd_msg(cmd_msg: JointTrajectory, joint_names): cmd_msg.joint_names = joint_names cmd_msg.points.append(JointTrajectoryPoint()) - cmd_msg.points[0].positions = [0.] * len(cmd_msg.joint_names) - cmd_msg.points[0].velocities = [0.] * len(cmd_msg.joint_names) + cmd_msg.points[0].positions = [0.0] * len(cmd_msg.joint_names) + cmd_msg.points[0].velocities = [0.0] * len(cmd_msg.joint_names) return cmd_msg -if __name__ == '__main__': - rospy.init_node('ros_control_gripper_bridge', anonymous=True) + +if __name__ == "__main__": + rospy.init_node("ros_control_gripper_bridge", anonymous=True) controller_gripper = str() - if rospy.has_param('~controller_gripper'): - controller_gripper = rospy.get_param('~controller_gripper') + if rospy.has_param("~controller_gripper"): + controller_gripper = rospy.get_param("~controller_gripper") else: rospy.logerr("controller_gripper private param is mandatory! Exiting.") exit() rospy.loginfo(f"controller_gripper: {controller_gripper}") controller_gripper_joints = [] - if rospy.has_param('~controller_gripper_joints'): - controller_gripper_joints = rospy.get_param('~controller_gripper_joints') + if rospy.has_param("~controller_gripper_joints"): + controller_gripper_joints = rospy.get_param("~controller_gripper_joints") else: rospy.logerr("controller_gripper_joints private param is mandatory! Exiting.") exit() rospy.loginfo(f"controller_gripper_joints: {controller_gripper_joints}") - if rospy.has_param('~GRIPPER_MAX'): - GRIPPER_MAX = rospy.get_param('~GRIPPER_MAX') - if rospy.has_param('~GRIPPER_MIN'): - GRIPPER_MIN = rospy.get_param('~GRIPPER_MIN') - if rospy.has_param('~TELEOP_MAX'): - TELEOP_MAX = rospy.get_param('~TELEOP_MAX') - if rospy.has_param('~TELEOP_MIN'): - TELEOP_MIN = rospy.get_param('~TELEOP_MIN') + if rospy.has_param("~GRIPPER_MAX"): + GRIPPER_MAX = rospy.get_param("~GRIPPER_MAX") + if rospy.has_param("~GRIPPER_MIN"): + GRIPPER_MIN = rospy.get_param("~GRIPPER_MIN") + if rospy.has_param("~TELEOP_MAX"): + TELEOP_MAX = rospy.get_param("~TELEOP_MAX") + if rospy.has_param("~TELEOP_MIN"): + TELEOP_MIN = rospy.get_param("~TELEOP_MIN") print_info() - gripper_cmd_pub = rospy.Publisher(controller_gripper + '/command', JointTrajectory, queue_size=1) + gripper_cmd_pub = rospy.Publisher( + controller_gripper + "/command", JointTrajectory, queue_size=1 + ) gripper_cmd_msg = init_cmd_msg(gripper_cmd_msg, controller_gripper_joints) rospy.Subscriber("teleop_gripper", Float32, io_callback) @@ -78,7 +87,3 @@ def init_cmd_msg(cmd_msg: JointTrajectory, joint_names): rate = rospy.Rate(rate_) while not rospy.is_shutdown(): rate.sleep() - - - - From a14567cec27c5e4f14e17bb2746073a399965797 Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Mon, 7 Oct 2024 17:19:38 +0200 Subject: [PATCH 29/56] Updated demo recorder --- launch/demo_recorder.launch | 2 +- python/demo_recorder.py | 118 +++++++++++++++++++++-------------- python/ros_control_bridge.py | 0 3 files changed, 73 insertions(+), 47 deletions(-) mode change 100644 => 100755 python/ros_control_bridge.py diff --git a/launch/demo_recorder.launch b/launch/demo_recorder.launch index c9cd31f..88691ae 100644 --- a/launch/demo_recorder.launch +++ b/launch/demo_recorder.launch @@ -1,6 +1,6 @@ "/home" - [0, 1] + [0] diff --git a/python/demo_recorder.py b/python/demo_recorder.py index be488a0..8df2ca7 100755 --- a/python/demo_recorder.py +++ b/python/demo_recorder.py @@ -44,16 +44,18 @@ def __init__(self): self.target_right_pose_data = np.zeros((1, 8)) # Init joint state recorder - self.joints_data = np.zeros((1, 28)) - rospy.Subscriber("/joint_states", JointState, self.cb_joint_state) + self.joints_data = np.zeros((1, 26)) + self.sub_joint_state = rospy.Subscriber( + "/joint_states", JointState, self.cb_joint_state + ) # Init left/right gripper recorder self.left_gripper_data = np.zeros((1, 3)) self.right_gripper_data = np.zeros((1, 3)) - rospy.Subscriber( + self.sub_left_gripper = rospy.Subscriber( "/gripper_left_controller/command", JointTrajectory, self.cb_left_gripper ) - rospy.Subscriber( + self.sub_right_gripper = rospy.Subscriber( "/gripper_right_controller/command", JointTrajectory, self.cb_right_gripper ) @@ -61,12 +63,17 @@ def __init__(self): self.tags_data = {} for tag in tags: self.tags_data[int(tag)] = np.zeros((1, 8)) - rospy.Subscriber(f"/pose_tag_{tag}", PoseStamped, self.cb_tag, (int(tag))) + rospy.Subscriber( + f"/inria_orbbec_tags/pose_tag_{tag}", + PoseStamped, + self.cb_tag, + (int(tag)), + ) def cb_joint_state(self, msg: JointState): - """Store joints state data (format: nPoints x [j1, ..., j27, time_stamp])""" - new_joints = np.zeros((1, 28)) - for i in range(27): + """Store joints state data (format: nPoints x [j1, ..., j25, time_stamp])""" + new_joints = np.zeros((1, 26)) + for i in range(25): new_joints[0, i] = msg.position[i] new_joints[0, -1] = msg.header.stamp.secs + 10**-9 * msg.header.stamp.nsecs self.joints_data = np.append(self.joints_data, new_joints, 0) @@ -101,28 +108,30 @@ def cb_right_gripper(self, msg: JointTrajectory): def cb_tag(self, msg: PoseStamped, args: int): """Store tags data (format: nTagsx [nPoints x [x, y, z, qx, qy, qz, qw, time_stamp]])""" - tag_id = int(args[0]) + tag_id = args new_tag = np.zeros((1, 8)) new_tag[0, 0] = msg.pose.position.x new_tag[0, 1] = msg.pose.position.y new_tag[0, 2] = msg.pose.position.z new_tag[0, 3] = msg.pose.position.x - new_tag[0, 4] = msg.pose.position.y - new_tag[0, 5] = msg.pose.position.z - new_tag[0, 6] = msg.pose.position.w + new_tag[0, 4] = msg.pose.orientation.y + new_tag[0, 5] = msg.pose.orientation.z + new_tag[0, 6] = msg.pose.orientation.w new_tag[0, 7] = msg.header.stamp.secs + 10**-9 * msg.header.stamp.nsecs self.tags_data[tag_id] = np.append(self.tags_data[tag_id], new_tag, 0) def record(self): """Query tf tree and store target and actual ee poses (format: nPoints x [x, y, z, qx, qy, qz, qw, time_stamp])""" + rospy.loginfo(f"Start recording") rate = rospy.Rate(100) while not rospy.is_shutdown(): - now = rospy.get_rostime() - try: # base-to-left-ee (actual) t = self.tf_buffer.lookup_transform( - "base_link", "gripper_left_grasping_frame", now + "base_link", + "gripper_left_grasping_frame", + rospy.Time(), + rospy.Duration(1.0), ) new_actual_left_pose = np.zeros((1, 8)) new_actual_left_pose[0, 1] = t.transform.translation.x @@ -132,7 +141,9 @@ def record(self): new_actual_left_pose[0, 5] = t.transform.rotation.y new_actual_left_pose[0, 6] = t.transform.rotation.z new_actual_left_pose[0, 7] = t.transform.rotation.w - new_actual_left_pose[0, 7] = now.to_sec() + new_actual_left_pose[0, 7] = ( + t.header.stamp.secs + 10**-9 * t.header.stamp.nsecs + ) self.actual_left_pose_data = np.append( self.actual_left_pose_data, new_actual_left_pose, 0 ) @@ -142,7 +153,10 @@ def record(self): try: # base-to-right-ee (actual) t = self.tf_buffer.lookup_transform( - "base_link", "gripper_right_grasping_frame", now + "base_link", + "gripper_right_grasping_frame", + rospy.Time(), + rospy.Duration(1.0), ) new_actual_right_pose = np.zeros((1, 8)) new_actual_right_pose[0, 1] = t.transform.translation.x @@ -152,7 +166,9 @@ def record(self): new_actual_right_pose[0, 5] = t.transform.rotation.y new_actual_right_pose[0, 6] = t.transform.rotation.z new_actual_right_pose[0, 7] = t.transform.rotation.w - new_actual_right_pose[0, 7] = now.to_sec() + new_actual_right_pose[0, 7] = ( + t.header.stamp.secs + 10**-9 * t.header.stamp.nsecs + ) self.actual_right_pose_data = np.append( self.actual_right_pose_data, new_actual_right_pose, 0 ) @@ -162,7 +178,10 @@ def record(self): try: # base-to-left-ee (target) t = self.tf_buffer.lookup_transform( - "ci/base_link", "ci/gripper_left_grasping_frame", now + "ci/base_link", + "ci/gripper_left_grasping_frame", + rospy.Time(), + rospy.Duration(1.0), ) new_target_left_pose = np.zeros((1, 8)) new_target_left_pose[0, 1] = t.transform.translation.x @@ -172,7 +191,9 @@ def record(self): new_target_left_pose[0, 5] = t.transform.rotation.y new_target_left_pose[0, 6] = t.transform.rotation.z new_target_left_pose[0, 7] = t.transform.rotation.w - new_target_left_pose[0, 7] = now.to_sec() + new_target_left_pose[0, 7] = ( + t.header.stamp.secs + 10**-9 * t.header.stamp.nsecs + ) self.target_left_pose_data = np.append( self.target_left_pose_data, new_target_left_pose, 0 ) @@ -182,7 +203,10 @@ def record(self): try: # base-to-right-ee (target) t = self.tf_buffer.lookup_transform( - "ci/base_link", "ci/gripper_right_grasping_frame", now + "ci/base_link", + "ci/gripper_right_grasping_frame", + rospy.Time(), + rospy.Duration(1.0), ) new_target_right_pose = np.zeros((1, 8)) new_target_right_pose[0, 1] = t.transform.translation.x @@ -192,7 +216,9 @@ def record(self): new_target_right_pose[0, 5] = t.transform.rotation.y new_target_right_pose[0, 6] = t.transform.rotation.z new_target_right_pose[0, 7] = t.transform.rotation.w - new_target_right_pose[0, 7] = now.to_sec() + new_target_right_pose[0, 7] = ( + t.header.stamp.secs + 10**-9 * t.header.stamp.nsecs + ) self.target_right_pose_data = np.append( self.target_right_pose_data, new_target_right_pose, 0 ) @@ -202,37 +228,37 @@ def record(self): rate.sleep() def dump_data(self): - rospy.logwarn(f"Dumping recorded data into '{self.save_folder}'") - np.savetxt( - f"{self.save_folder}/actual_left_pose_data.csv", - self.actual_left_pose_data, - delimiter=",", + self.sub_joint_state.unregister() + self.sub_left_gripper.unregister() + self.sub_right_gripper.unregister() + + rospy.logwarn( + f"Finished recording, saving recorded data into '{self.save_folder}'..." + ) + + np.save(f"{self.save_folder}/joints_data.npy", self.joints_data) + np.save( + f"{self.save_folder}/actual_left_pose_data.npy", self.actual_left_pose_data ) - np.savetxt( - f"{self.save_folder}/actual_right_pose_data.csv", + np.save( + f"{self.save_folder}/actual_right_pose_data.npy", self.actual_right_pose_data, - delimiter=",", ) - np.savetxt( - f"{self.save_folder}/target_left_pose_data.csv", - self.target_left_pose_data, - delimiter=",", + np.save( + f"{self.save_folder}/target_left_pose_data.npy", self.target_left_pose_data ) - np.savetxt( - f"{self.save_folder}/target_right_pose_data.csv", + np.save( + f"{self.save_folder}/target_right_pose_data.npy", self.target_right_pose_data, - delimiter=",", ) - np.savetxt( - f"{self.save_folder}/left_gripper_data.csv", - self.left_gripper_data, - delimiter=",", - ) - np.savetxt( - f"{self.save_folder}/right_gripper_data.csv", - self.right_gripper_data, - delimiter=",", + np.save( + f"{self.save_folder}/target_right_pose_data.npy", + self.target_right_pose_data, ) + np.save(f"{self.save_folder}/left_gripper_data.npy", self.left_gripper_data) + np.save(f"{self.save_folder}/right_gripper_data.npy", self.right_gripper_data) + for tag in self.tags_data: + np.save(f"{self.save_folder}/pose_tag_{tag}.npy", self.tags_data[tag]) for tag in self.tags_data: np.savetxt( diff --git a/python/ros_control_bridge.py b/python/ros_control_bridge.py old mode 100644 new mode 100755 From 1a7672456056f6784fb5c62e93953984364a5595 Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Mon, 7 Oct 2024 18:12:03 +0200 Subject: [PATCH 30/56] Fixed recorder and added dummy postprocessing script --- .gitignore | 2 + launch/demo_recorder.launch | 2 +- python/demo/postprocess_demo.py | 43 +++++++++++++++ python/demo_recorder.py | 94 ++++++++++++++------------------- 4 files changed, 86 insertions(+), 55 deletions(-) create mode 100644 .gitignore create mode 100644 python/demo/postprocess_demo.py diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..05676d1 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +*.npy +*.csv diff --git a/launch/demo_recorder.launch b/launch/demo_recorder.launch index 88691ae..7364768 100644 --- a/launch/demo_recorder.launch +++ b/launch/demo_recorder.launch @@ -1,6 +1,6 @@ - "/home" + "/home/forest_ws/src/tiago_dual_cartesio_config/python/demo" [0] diff --git a/python/demo/postprocess_demo.py b/python/demo/postprocess_demo.py new file mode 100644 index 0000000..1da9523 --- /dev/null +++ b/python/demo/postprocess_demo.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python3 + +import numpy as np +import matplotlib.pyplot as plt + +actual_left_pose = np.load("data_actual_left_pose.npy")[1:, :] +actual_right_pose = np.load("data_actual_right_pose.npy")[1:, :] +target_left_pose = np.load("data_target_left_pose.npy")[1:, :] +target_right_pose = np.load("data_target_right_pose.npy")[1:, :] +right_gripper = np.load("data_right_gripper.npy")[1:, :] +left_gripper = np.load("data_left_gripper.npy")[1:, :] +pose_tag_0 = np.load("data_pose_tag_0.npy")[1:, :] + + +fig = plt.figure() +fig.suptitle("Left EE", fontsize=16) +plt.plot( + target_left_pose[:, -1] - target_left_pose[0, -1], + target_left_pose[:, 0], + linestyle="--", + color="tab:red", + linewidth=2, + label="target x", +) +plt.plot( + target_left_pose[:, -1] - target_left_pose[0, -1], + target_left_pose[:, 1], + linestyle="--", + color="tab:green", + linewidth=2, + label="target y", +) +plt.plot( + target_left_pose[:, -1] - target_left_pose[0, -1], + target_left_pose[:, 2], + color="tab:blue", + linestyle="--", + linewidth=2, + label="target z", +) +plt.legend() +plt.grid() +plt.show() diff --git a/python/demo_recorder.py b/python/demo_recorder.py index 8df2ca7..6d6316f 100755 --- a/python/demo_recorder.py +++ b/python/demo_recorder.py @@ -28,7 +28,7 @@ def __init__(self): tags = [] # Set dump file on node shutdown - rospy.on_shutdown(self.dump_data) + rospy.on_shutdown(self.save_data) # Set up tf2 listener self.tf_buffer = tf2_ros.Buffer() @@ -38,20 +38,20 @@ def __init__(self): rospy.sleep(1) # Init ee pose recorder - self.actual_left_pose_data = np.zeros((1, 8)) - self.actual_right_pose_data = np.zeros((1, 8)) - self.target_left_pose_data = np.zeros((1, 8)) - self.target_right_pose_data = np.zeros((1, 8)) + self.data_actual_left_pose = np.zeros((1, 8)) + self.data_actual_right_pose = np.zeros((1, 8)) + self.data_target_left_pose = np.zeros((1, 8)) + self.data_target_right_pose = np.zeros((1, 8)) # Init joint state recorder - self.joints_data = np.zeros((1, 26)) + self.data_joints = np.zeros((1, 26)) self.sub_joint_state = rospy.Subscriber( "/joint_states", JointState, self.cb_joint_state ) # Init left/right gripper recorder - self.left_gripper_data = np.zeros((1, 3)) - self.right_gripper_data = np.zeros((1, 3)) + self.data_left_gripper = np.zeros((1, 3)) + self.data_right_gripper = np.zeros((1, 3)) self.sub_left_gripper = rospy.Subscriber( "/gripper_left_controller/command", JointTrajectory, self.cb_left_gripper ) @@ -60,9 +60,9 @@ def __init__(self): ) # Init recorder(s) for april tags - self.tags_data = {} + self.data_tags = {} for tag in tags: - self.tags_data[int(tag)] = np.zeros((1, 8)) + self.data_tags[int(tag)] = np.zeros((1, 8)) rospy.Subscriber( f"/inria_orbbec_tags/pose_tag_{tag}", PoseStamped, @@ -70,13 +70,15 @@ def __init__(self): (int(tag)), ) + rospy.loginfo(f"Demo recorder initialized") + def cb_joint_state(self, msg: JointState): """Store joints state data (format: nPoints x [j1, ..., j25, time_stamp])""" new_joints = np.zeros((1, 26)) for i in range(25): new_joints[0, i] = msg.position[i] new_joints[0, -1] = msg.header.stamp.secs + 10**-9 * msg.header.stamp.nsecs - self.joints_data = np.append(self.joints_data, new_joints, 0) + self.data_joints = np.append(self.data_joints, new_joints, 0) def cb_left_gripper(self, msg: JointTrajectory): """Store left gripper data (format: nPoints x [joint_left, joint_right, time_stamp])""" @@ -88,7 +90,7 @@ def cb_left_gripper(self, msg: JointTrajectory): msg.joint_names.index("gripper_left_right_finger_joint") ] new_left_gripper[0, 2] = msg.header.stamp.secs + 10**-9 * msg.header.stamp.nsecs - self.left_gripper_data = np.append(self.left_gripper_data, new_left_gripper, 0) + self.data_left_gripper = np.append(self.data_left_gripper, new_left_gripper, 0) def cb_right_gripper(self, msg: JointTrajectory): """Store right gripper data (format: nPoints x [joint_left, joint_right, time_stamp])""" @@ -102,8 +104,8 @@ def cb_right_gripper(self, msg: JointTrajectory): new_right_gripper[0, 2] = ( msg.header.stamp.secs + 10**-9 * msg.header.stamp.nsecs ) - self.right_gripper_data = np.append( - self.right_gripper_data, new_right_gripper, 0 + self.data_right_gripper = np.append( + self.data_right_gripper, new_right_gripper, 0 ) def cb_tag(self, msg: PoseStamped, args: int): @@ -118,11 +120,10 @@ def cb_tag(self, msg: PoseStamped, args: int): new_tag[0, 5] = msg.pose.orientation.z new_tag[0, 6] = msg.pose.orientation.w new_tag[0, 7] = msg.header.stamp.secs + 10**-9 * msg.header.stamp.nsecs - self.tags_data[tag_id] = np.append(self.tags_data[tag_id], new_tag, 0) + self.data_tags[tag_id] = np.append(self.data_tags[tag_id], new_tag, 0) def record(self): """Query tf tree and store target and actual ee poses (format: nPoints x [x, y, z, qx, qy, qz, qw, time_stamp])""" - rospy.loginfo(f"Start recording") rate = rospy.Rate(100) while not rospy.is_shutdown(): try: @@ -131,7 +132,6 @@ def record(self): "base_link", "gripper_left_grasping_frame", rospy.Time(), - rospy.Duration(1.0), ) new_actual_left_pose = np.zeros((1, 8)) new_actual_left_pose[0, 1] = t.transform.translation.x @@ -144,8 +144,8 @@ def record(self): new_actual_left_pose[0, 7] = ( t.header.stamp.secs + 10**-9 * t.header.stamp.nsecs ) - self.actual_left_pose_data = np.append( - self.actual_left_pose_data, new_actual_left_pose, 0 + self.data_actual_left_pose = np.append( + self.data_actual_left_pose, new_actual_left_pose, 0 ) except Exception as err: rospy.logwarn(err) @@ -156,7 +156,6 @@ def record(self): "base_link", "gripper_right_grasping_frame", rospy.Time(), - rospy.Duration(1.0), ) new_actual_right_pose = np.zeros((1, 8)) new_actual_right_pose[0, 1] = t.transform.translation.x @@ -169,8 +168,8 @@ def record(self): new_actual_right_pose[0, 7] = ( t.header.stamp.secs + 10**-9 * t.header.stamp.nsecs ) - self.actual_right_pose_data = np.append( - self.actual_right_pose_data, new_actual_right_pose, 0 + self.data_actual_right_pose = np.append( + self.data_actual_right_pose, new_actual_right_pose, 0 ) except Exception as err: rospy.logwarn(err) @@ -181,7 +180,6 @@ def record(self): "ci/base_link", "ci/gripper_left_grasping_frame", rospy.Time(), - rospy.Duration(1.0), ) new_target_left_pose = np.zeros((1, 8)) new_target_left_pose[0, 1] = t.transform.translation.x @@ -194,8 +192,8 @@ def record(self): new_target_left_pose[0, 7] = ( t.header.stamp.secs + 10**-9 * t.header.stamp.nsecs ) - self.target_left_pose_data = np.append( - self.target_left_pose_data, new_target_left_pose, 0 + self.data_target_left_pose = np.append( + self.data_target_left_pose, new_target_left_pose, 0 ) except Exception as err: rospy.logwarn(err) @@ -206,7 +204,6 @@ def record(self): "ci/base_link", "ci/gripper_right_grasping_frame", rospy.Time(), - rospy.Duration(1.0), ) new_target_right_pose = np.zeros((1, 8)) new_target_right_pose[0, 1] = t.transform.translation.x @@ -219,53 +216,42 @@ def record(self): new_target_right_pose[0, 7] = ( t.header.stamp.secs + 10**-9 * t.header.stamp.nsecs ) - self.target_right_pose_data = np.append( - self.target_right_pose_data, new_target_right_pose, 0 + self.data_target_right_pose = np.append( + self.data_target_right_pose, new_target_right_pose, 0 ) except Exception as err: rospy.logwarn(err) rate.sleep() - def dump_data(self): - self.sub_joint_state.unregister() - self.sub_left_gripper.unregister() - self.sub_right_gripper.unregister() - - rospy.logwarn( + def save_data(self): + rospy.loginfo( f"Finished recording, saving recorded data into '{self.save_folder}'..." ) - np.save(f"{self.save_folder}/joints_data.npy", self.joints_data) + np.save(f"{self.save_folder}/data_joints.npy", self.data_joints) np.save( - f"{self.save_folder}/actual_left_pose_data.npy", self.actual_left_pose_data + f"{self.save_folder}/data_actual_left_pose.npy", self.data_actual_left_pose ) np.save( - f"{self.save_folder}/actual_right_pose_data.npy", - self.actual_right_pose_data, + f"{self.save_folder}/data_actual_right_pose.npy", + self.data_actual_right_pose, ) np.save( - f"{self.save_folder}/target_left_pose_data.npy", self.target_left_pose_data + f"{self.save_folder}/data_target_left_pose.npy", self.data_target_left_pose ) np.save( - f"{self.save_folder}/target_right_pose_data.npy", - self.target_right_pose_data, + f"{self.save_folder}/data_target_right_pose.npy", + self.data_target_right_pose, ) np.save( - f"{self.save_folder}/target_right_pose_data.npy", - self.target_right_pose_data, + f"{self.save_folder}/data_target_right_pose.npy", + self.data_target_right_pose, ) - np.save(f"{self.save_folder}/left_gripper_data.npy", self.left_gripper_data) - np.save(f"{self.save_folder}/right_gripper_data.npy", self.right_gripper_data) - for tag in self.tags_data: - np.save(f"{self.save_folder}/pose_tag_{tag}.npy", self.tags_data[tag]) - - for tag in self.tags_data: - np.savetxt( - f"{self.save_folder}/pose_tag_{tag}.csv", - self.tags_data[tag], - delimiter=",", - ) + np.save(f"{self.save_folder}/data_left_gripper.npy", self.data_left_gripper) + np.save(f"{self.save_folder}/data_right_gripper.npy", self.data_right_gripper) + for tag in self.data_tags: + np.save(f"{self.save_folder}/data_pose_tag_{tag}.npy", self.data_tags[tag]) if __name__ == "__main__": From d2ba54d50b885a87fc14f8944387378a0ba75630 Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Mon, 7 Oct 2024 18:33:23 +0200 Subject: [PATCH 31/56] Added param for setting tf record rate (and fixed indices) --- launch/demo_recorder.launch | 1 + python/demo/postprocess_demo.py | 30 +++++++++++++++ python/demo_recorder.py | 65 ++++++++++++++++++--------------- 3 files changed, 66 insertions(+), 30 deletions(-) diff --git a/launch/demo_recorder.launch b/launch/demo_recorder.launch index 7364768..9e44861 100644 --- a/launch/demo_recorder.launch +++ b/launch/demo_recorder.launch @@ -2,5 +2,6 @@ "/home/forest_ws/src/tiago_dual_cartesio_config/python/demo" [0] + 100 diff --git a/python/demo/postprocess_demo.py b/python/demo/postprocess_demo.py index 1da9523..ea28e5c 100644 --- a/python/demo/postprocess_demo.py +++ b/python/demo/postprocess_demo.py @@ -41,3 +41,33 @@ plt.legend() plt.grid() plt.show() + +fig = plt.figure() +fig.suptitle("Right EE", fontsize=16) +plt.plot( + target_right_pose[:, -1] - target_right_pose[0, -1], + target_right_pose[:, 0], + linestyle="--", + color="tab:red", + linewidth=2, + label="target x", +) +plt.plot( + target_right_pose[:, -1] - target_right_pose[0, -1], + target_right_pose[:, 1], + linestyle="--", + color="tab:green", + linewidth=2, + label="target y", +) +plt.plot( + target_right_pose[:, -1] - target_right_pose[0, -1], + target_right_pose[:, 2], + color="tab:blue", + linestyle="--", + linewidth=2, + label="target z", +) +plt.legend() +plt.grid() +plt.show() diff --git a/python/demo_recorder.py b/python/demo_recorder.py index 6d6316f..ed37abf 100755 --- a/python/demo_recorder.py +++ b/python/demo_recorder.py @@ -21,7 +21,10 @@ def __init__(self): self.save_folder = rospy.get_param("~save_folder") else: self.save_folder = os.environ["HOME"] - + if rospy.has_param("~tf_hz"): + self.tf_hz = rospy.get_param("~tf_hz") + else: + self.tf_hz = 100 if rospy.has_param("~tags"): tags = rospy.get_param("~tags") else: @@ -124,7 +127,7 @@ def cb_tag(self, msg: PoseStamped, args: int): def record(self): """Query tf tree and store target and actual ee poses (format: nPoints x [x, y, z, qx, qy, qz, qw, time_stamp])""" - rate = rospy.Rate(100) + rate = rospy.Rate(self.tf_hz) while not rospy.is_shutdown(): try: # base-to-left-ee (actual) @@ -134,13 +137,13 @@ def record(self): rospy.Time(), ) new_actual_left_pose = np.zeros((1, 8)) - new_actual_left_pose[0, 1] = t.transform.translation.x - new_actual_left_pose[0, 2] = t.transform.translation.y - new_actual_left_pose[0, 3] = t.transform.translation.z - new_actual_left_pose[0, 4] = t.transform.rotation.x - new_actual_left_pose[0, 5] = t.transform.rotation.y - new_actual_left_pose[0, 6] = t.transform.rotation.z - new_actual_left_pose[0, 7] = t.transform.rotation.w + new_actual_left_pose[0, 0] = t.transform.translation.x + new_actual_left_pose[0, 1] = t.transform.translation.y + new_actual_left_pose[0, 2] = t.transform.translation.z + new_actual_left_pose[0, 3] = t.transform.rotation.x + new_actual_left_pose[0, 4] = t.transform.rotation.y + new_actual_left_pose[0, 5] = t.transform.rotation.z + new_actual_left_pose[0, 6] = t.transform.rotation.w new_actual_left_pose[0, 7] = ( t.header.stamp.secs + 10**-9 * t.header.stamp.nsecs ) @@ -158,13 +161,13 @@ def record(self): rospy.Time(), ) new_actual_right_pose = np.zeros((1, 8)) - new_actual_right_pose[0, 1] = t.transform.translation.x - new_actual_right_pose[0, 2] = t.transform.translation.y - new_actual_right_pose[0, 3] = t.transform.translation.z - new_actual_right_pose[0, 4] = t.transform.rotation.x - new_actual_right_pose[0, 5] = t.transform.rotation.y - new_actual_right_pose[0, 6] = t.transform.rotation.z - new_actual_right_pose[0, 7] = t.transform.rotation.w + new_actual_right_pose[0, 0] = t.transform.translation.x + new_actual_right_pose[0, 1] = t.transform.translation.y + new_actual_right_pose[0, 2] = t.transform.translation.z + new_actual_right_pose[0, 3] = t.transform.rotation.x + new_actual_right_pose[0, 4] = t.transform.rotation.y + new_actual_right_pose[0, 5] = t.transform.rotation.z + new_actual_right_pose[0, 6] = t.transform.rotation.w new_actual_right_pose[0, 7] = ( t.header.stamp.secs + 10**-9 * t.header.stamp.nsecs ) @@ -182,13 +185,13 @@ def record(self): rospy.Time(), ) new_target_left_pose = np.zeros((1, 8)) - new_target_left_pose[0, 1] = t.transform.translation.x - new_target_left_pose[0, 2] = t.transform.translation.y - new_target_left_pose[0, 3] = t.transform.translation.z - new_target_left_pose[0, 4] = t.transform.rotation.x - new_target_left_pose[0, 5] = t.transform.rotation.y - new_target_left_pose[0, 6] = t.transform.rotation.z - new_target_left_pose[0, 7] = t.transform.rotation.w + new_target_left_pose[0, 0] = t.transform.translation.x + new_target_left_pose[0, 1] = t.transform.translation.y + new_target_left_pose[0, 2] = t.transform.translation.z + new_target_left_pose[0, 3] = t.transform.rotation.x + new_target_left_pose[0, 4] = t.transform.rotation.y + new_target_left_pose[0, 5] = t.transform.rotation.z + new_target_left_pose[0, 6] = t.transform.rotation.w new_target_left_pose[0, 7] = ( t.header.stamp.secs + 10**-9 * t.header.stamp.nsecs ) @@ -206,13 +209,13 @@ def record(self): rospy.Time(), ) new_target_right_pose = np.zeros((1, 8)) - new_target_right_pose[0, 1] = t.transform.translation.x - new_target_right_pose[0, 2] = t.transform.translation.y - new_target_right_pose[0, 3] = t.transform.translation.z - new_target_right_pose[0, 4] = t.transform.rotation.x - new_target_right_pose[0, 5] = t.transform.rotation.y - new_target_right_pose[0, 6] = t.transform.rotation.z - new_target_right_pose[0, 7] = t.transform.rotation.w + new_target_right_pose[0, 0] = t.transform.translation.x + new_target_right_pose[0, 1] = t.transform.translation.y + new_target_right_pose[0, 2] = t.transform.translation.z + new_target_right_pose[0, 3] = t.transform.rotation.x + new_target_right_pose[0, 4] = t.transform.rotation.y + new_target_right_pose[0, 5] = t.transform.rotation.z + new_target_right_pose[0, 6] = t.transform.rotation.w new_target_right_pose[0, 7] = ( t.header.stamp.secs + 10**-9 * t.header.stamp.nsecs ) @@ -253,6 +256,8 @@ def save_data(self): for tag in self.data_tags: np.save(f"{self.save_folder}/data_pose_tag_{tag}.npy", self.data_tags[tag]) + rospy.loginfo(f"Saving complete") + if __name__ == "__main__": dr = DemoRecorder() From be682cca4fbd77cba595927b6a5aaa818ea356cd Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Tue, 8 Oct 2024 12:45:42 +0200 Subject: [PATCH 32/56] Saving into date-time-stamped folders --- launch/demo_recorder.launch | 2 +- python/demo/postprocess_demo.py | 20 ++++++++++++-------- python/demo_recorder.py | 27 +++++++++++++++++++-------- 3 files changed, 32 insertions(+), 17 deletions(-) diff --git a/launch/demo_recorder.launch b/launch/demo_recorder.launch index 9e44861..b3ffc57 100644 --- a/launch/demo_recorder.launch +++ b/launch/demo_recorder.launch @@ -1,6 +1,6 @@ - "/home/forest_ws/src/tiago_dual_cartesio_config/python/demo" + "/home/forest_ws/src/tiago_dual_cartesio_config/python/demo" [0] 100 diff --git a/python/demo/postprocess_demo.py b/python/demo/postprocess_demo.py index ea28e5c..6ff8731 100644 --- a/python/demo/postprocess_demo.py +++ b/python/demo/postprocess_demo.py @@ -1,15 +1,19 @@ #!/usr/bin/env python3 -import numpy as np import matplotlib.pyplot as plt +import numpy as np +import os + +subf = "" + -actual_left_pose = np.load("data_actual_left_pose.npy")[1:, :] -actual_right_pose = np.load("data_actual_right_pose.npy")[1:, :] -target_left_pose = np.load("data_target_left_pose.npy")[1:, :] -target_right_pose = np.load("data_target_right_pose.npy")[1:, :] -right_gripper = np.load("data_right_gripper.npy")[1:, :] -left_gripper = np.load("data_left_gripper.npy")[1:, :] -pose_tag_0 = np.load("data_pose_tag_0.npy")[1:, :] +actual_left_pose = np.load(os.path.join(subf, "data_actual_left_pose.npy"))[1:, :] +actual_right_pose = np.load(os.path.join(subf, "data_actual_right_pose.npy"))[1:, :] +target_left_pose = np.load(os.path.join(subf, "data_target_left_pose.npy"))[1:, :] +target_right_pose = np.load(os.path.join(subf, "data_target_right_pose.npy"))[1:, :] +right_gripper = np.load(os.path.join(subf, "data_right_gripper.npy"))[1:, :] +left_gripper = np.load(os.path.join(subf, "data_left_gripper.npy"))[1:, :] +pose_tag_0 = np.load(os.path.join(subf, "data_pose_tag_0.npy"))[1:, :] fig = plt.figure() diff --git a/python/demo_recorder.py b/python/demo_recorder.py index ed37abf..bcb4e9d 100755 --- a/python/demo_recorder.py +++ b/python/demo_recorder.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 +from datetime import datetime import numpy as np import threading import os @@ -17,25 +18,30 @@ def __init__(self): rospy.init_node("demo_recorder") # Load params - if rospy.has_param("~save_folder"): - self.save_folder = rospy.get_param("~save_folder") + if rospy.has_param("~tags"): + tags = rospy.get_param("~tags") else: - self.save_folder = os.environ["HOME"] + tags = [] + if rospy.has_param("~folder_path"): + folder_path = rospy.get_param("~folder_path") + else: + folder_path = os.environ["HOME"] if rospy.has_param("~tf_hz"): self.tf_hz = rospy.get_param("~tf_hz") else: self.tf_hz = 100 - if rospy.has_param("~tags"): - tags = rospy.get_param("~tags") - else: - tags = [] + + now = datetime.now() # current date and time + date_time = now.strftime("%m-%d-%Y_%H-%M-%S") + self.save_folder = os.path.join(folder_path, date_time) + os.mkdir(self.save_folder) # Set dump file on node shutdown rospy.on_shutdown(self.save_data) # Set up tf2 listener self.tf_buffer = tf2_ros.Buffer() - tf2_ros.TransformListener(self.tf_buffer) + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) # Wait for tf listener rospy.sleep(1) @@ -228,6 +234,11 @@ def record(self): rate.sleep() def save_data(self): + self.tf_listener.unregister() + self.sub_joint_state.unregister() + self.sub_left_gripper.unregister() + self.sub_right_gripper.unregister() + rospy.loginfo( f"Finished recording, saving recorded data into '{self.save_folder}'..." ) From 150027bdccc62ccd183de1eb33a4e1d7c407eff9 Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Thu, 10 Oct 2024 13:32:48 +0200 Subject: [PATCH 33/56] Re-enabled arm_left_7_joint --- stack/tiago_dual.stack | 3 --- 1 file changed, 3 deletions(-) diff --git a/stack/tiago_dual.stack b/stack/tiago_dual.stack index 7f9c11f..3467373 100644 --- a/stack/tiago_dual.stack +++ b/stack/tiago_dual.stack @@ -14,8 +14,6 @@ Lee: distal_link: "gripper_left_grasping_frame" base_link: "base_link" lambda: 0.1 - disabled_joints: - - "arm_left_7_joint" Ree: type: "Cartesian" @@ -38,7 +36,6 @@ Postural: wheel_front_right_joint: 0.0 wheel_rear_left_joint: 0.0 wheel_rear_right_joint: 0.0 - arm_left_7_joint: 0.0 Base: type: "Cartesian" From 62932b0bc11e81351bbf54aaf5f680b7b67fdfcb Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Fri, 11 Oct 2024 13:44:51 +0200 Subject: [PATCH 34/56] Added ft emergency stop --- python/ros_control_bridge.py | 200 +++++++++++++++++++++-------------- 1 file changed, 121 insertions(+), 79 deletions(-) diff --git a/python/ros_control_bridge.py b/python/ros_control_bridge.py index c8f80aa..c8d23cf 100755 --- a/python/ros_control_bridge.py +++ b/python/ros_control_bridge.py @@ -4,30 +4,33 @@ import os from sensor_msgs.msg import JointState from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint -from geometry_msgs.msg import Twist +from geometry_msgs.msg import Twist, WrenchStamped from std_msgs.msg import Float32, Bool from std_srvs.srv import SetBool, SetBoolResponse -head_cmd_pub = None -head_cmd_msg = JointTrajectory() +head_cmd_pub_ = None +head_cmd_msg_ = JointTrajectory() -left_arm_cmd_pub = None -left_arm_cmd_msg = JointTrajectory() +left_arm_cmd_pub_ = None +left_arm_cmd_msg_ = JointTrajectory() -right_arm_cmd_pub = None -right_arm_cmd_msg = JointTrajectory() +right_arm_cmd_pub_ = None +right_arm_cmd_msg_ = JointTrajectory() -torso_cmd_pub = None -torso_cmd_msg = JointTrajectory() +torso_cmd_pub_ = None +torso_cmd_msg_ = JointTrajectory() -mobile_base_cmd_pub = None -mobile_base_cmd_msg = Twist() +mobile_base_cmd_pub_ = None +mobile_base_cmd_msg_ = Twist() -time_from_start = 0.2 +time_from_start_ = 0.2 + +wrist_force_limit = 20 +wrist_torque_limit = 2 exit_ = False -send_commands = True +send_commands_ = True SEND_VELOCITY = False @@ -52,7 +55,7 @@ def set_initial_configuration(data: JointState): def fill_cms_msg(data: JointState, cmd_msg: JointTrajectory, send_velocity=True): - global time_from_start + global time_from_start_ for joint_name in cmd_msg.joint_names: cmd_msg.points[0].positions[cmd_msg.joint_names.index(joint_name)] = ( data.position[data.name.index(joint_name)] @@ -61,46 +64,76 @@ def fill_cms_msg(data: JointState, cmd_msg: JointTrajectory, send_velocity=True) cmd_msg.points[0].velocities[cmd_msg.joint_names.index(joint_name)] = ( data.velocity[data.name.index(joint_name)] ) - cmd_msg.points[0].time_from_start = rospy.Duration(time_from_start) + cmd_msg.points[0].time_from_start_ = rospy.Duration(time_from_start_) return cmd_msg +def left_ft_callback(data: WrenchStamped): + global wrist_force_limit + global wrist_torque_limit + global send_commands_ + if ( + data.wrench.force.x > wrist_force_limit + or data.wrench.force.y > wrist_force_limit + or data.wrench.force.z > wrist_force_limit + ): + rospy.logwarn(f"Exceeded wrist force limit. Motion disabled") + send_commands_ = False + + if ( + data.wrench.torque.x > wrist_torque_limit + or data.wrench.torque.y > wrist_torque_limit + or data.wrench.torque.z > wrist_torque_limit + ): + rospy.logwarn(f"Exceeded wrist torque limit. Motion disabled") + send_commands_ = False + + +def right_ft_callback(data: WrenchStamped): + global wrist_force_limit + global wrist_torque_limit + global send_commands_ + pass + + def io_callback(data: JointState): - global head_cmd_pub, head_cmd_msg - global left_arm_cmd_pub, left_arm_cmd_msg - global right_arm_cmd_pub, right_arm_cmd_msg - global torso_cmd_pub, torso_cmd_msg - global mobile_base_cmd_pub, mobile_base_cmd_msg - global time_from_start - global send_commands - - head_cmd_msg = fill_cms_msg(data, head_cmd_msg, send_velocity=SEND_VELOCITY) - left_arm_cmd_msg = fill_cms_msg(data, left_arm_cmd_msg, send_velocity=SEND_VELOCITY) - right_arm_cmd_msg = fill_cms_msg( - data, right_arm_cmd_msg, send_velocity=SEND_VELOCITY + global head_cmd_pub_, head_cmd_msg_ + global left_arm_cmd_pub_, left_arm_cmd_msg_ + global right_arm_cmd_pub_, right_arm_cmd_msg_ + global torso_cmd_pub_, torso_cmd_msg_ + global mobile_base_cmd_pub_, mobile_base_cmd_msg_ + global time_from_start_ + global send_commands_ + + head_cmd_msg_ = fill_cms_msg(data, head_cmd_msg_, send_velocity=SEND_VELOCITY) + left_arm_cmd_msg_ = fill_cms_msg( + data, left_arm_cmd_msg_, send_velocity=SEND_VELOCITY + ) + right_arm_cmd_msg_ = fill_cms_msg( + data, right_arm_cmd_msg_, send_velocity=SEND_VELOCITY ) - torso_cmd_msg = fill_cms_msg(data, torso_cmd_msg, send_velocity=SEND_VELOCITY) + torso_cmd_msg_ = fill_cms_msg(data, torso_cmd_msg_, send_velocity=SEND_VELOCITY) - mobile_base_cmd_msg.linear.x = data.velocity[0] - mobile_base_cmd_msg.linear.y = data.velocity[1] - mobile_base_cmd_msg.linear.z = 0.0 - mobile_base_cmd_msg.angular.x = 0.0 - mobile_base_cmd_msg.angular.y = 0.0 - mobile_base_cmd_msg.angular.z = data.velocity[5] + mobile_base_cmd_msg_.linear.x = data.velocity[0] + mobile_base_cmd_msg_.linear.y = data.velocity[1] + mobile_base_cmd_msg_.linear.z = 0.0 + mobile_base_cmd_msg_.angular.x = 0.0 + mobile_base_cmd_msg_.angular.y = 0.0 + mobile_base_cmd_msg_.angular.z = data.velocity[5] time = rospy.get_rostime() - head_cmd_msg.header.stamp = time - left_arm_cmd_msg.header.stamp = time - right_arm_cmd_msg.header.stamp = time - torso_cmd_msg.header.stamp = time + head_cmd_msg_.header.stamp = time + left_arm_cmd_msg_.header.stamp = time + right_arm_cmd_msg_.header.stamp = time + torso_cmd_msg_.header.stamp = time - if send_commands: - head_cmd_pub.publish(head_cmd_msg) - left_arm_cmd_pub.publish(left_arm_cmd_msg) - right_arm_cmd_pub.publish(right_arm_cmd_msg) - torso_cmd_pub.publish(torso_cmd_msg) - mobile_base_cmd_pub.publish(mobile_base_cmd_msg) + if send_commands_: + head_cmd_pub_.publish(head_cmd_msg_) + left_arm_cmd_pub_.publish(left_arm_cmd_msg_) + right_arm_cmd_pub_.publish(right_arm_cmd_msg_) + torso_cmd_pub_.publish(torso_cmd_msg_) + mobile_base_cmd_pub_.publish(mobile_base_cmd_msg_) def init_cmd_msg(cmd_msg: JointTrajectory, joint_names): @@ -111,25 +144,25 @@ def init_cmd_msg(cmd_msg: JointTrajectory, joint_names): return cmd_msg -def set_send_commands(req: SetBool): - global send_commands - send_commands = req.data +def set_send_commands_(req: SetBool): + global send_commands_ + send_commands_ = req.data response = SetBoolResponse() response.success = True - response.message = "send_commands set to " + str(req.data) - rospy.loginfo(f"Setting send_commands={req.data}") + response.message = f"send_commands_: {req.data}" + rospy.loginfo(f"Setting 'send_commands_: {req.data}'") return response -def time_from_start_cb(msg: Float32): - global time_from_start - if msg.data != time_from_start: +def time_from_start__cb(msg: Float32): + global time_from_start_ + if msg.data != time_from_start_: if msg.data >= 0.1: - time_from_start = msg.data - rospy.loginfo(f"Setting time_from_start={time_from_start}") + time_from_start_ = msg.data + rospy.loginfo(f"Setting 'time_from_start_: {time_from_start_}'") else: rospy.logwarn( - "Trying to set a 'time_from_start' too low. It must be >= 0.1secs" + "Trying to set a too low 'time_from_start_'. It must be >= 0.1secs" ) @@ -148,16 +181,16 @@ def control_initiator_cb(msg): set_initial_configuration(data) # Set up command publishers and msgs - head_cmd_pub = rospy.Publisher( + head_cmd_pub_ = rospy.Publisher( "head_controller/command", JointTrajectory, queue_size=1 ) - head_cmd_msg = init_cmd_msg(head_cmd_msg, ["head_1_joint", "head_2_joint"]) + head_cmd_msg_ = init_cmd_msg(head_cmd_msg_, ["head_1_joint", "head_2_joint"]) - left_arm_cmd_pub = rospy.Publisher( + left_arm_cmd_pub_ = rospy.Publisher( "/arm_left_controller/command", JointTrajectory, queue_size=1 ) - left_arm_cmd_msg = init_cmd_msg( - left_arm_cmd_msg, + left_arm_cmd_msg_ = init_cmd_msg( + left_arm_cmd_msg_, [ "arm_left_1_joint", "arm_left_2_joint", @@ -169,11 +202,11 @@ def control_initiator_cb(msg): ], ) - right_arm_cmd_pub = rospy.Publisher( + right_arm_cmd_pub_ = rospy.Publisher( "/arm_right_controller/command", JointTrajectory, queue_size=1 ) - right_arm_cmd_msg = init_cmd_msg( - right_arm_cmd_msg, + right_arm_cmd_msg_ = init_cmd_msg( + right_arm_cmd_msg_, [ "arm_right_1_joint", "arm_right_2_joint", @@ -185,26 +218,28 @@ def control_initiator_cb(msg): ], ) - torso_cmd_pub = rospy.Publisher( + torso_cmd_pub_ = rospy.Publisher( "/torso_controller/command", JointTrajectory, queue_size=1 ) - torso_cmd_msg = init_cmd_msg(torso_cmd_msg, ["torso_lift_joint"]) + torso_cmd_msg_ = init_cmd_msg(torso_cmd_msg_, ["torso_lift_joint"]) - mobile_base_cmd_pub = rospy.Publisher( + mobile_base_cmd_pub_ = rospy.Publisher( "/mobile_base_controller/cmd_vel", Twist, queue_size=1 ) # Get (possible) enable/disable parameter and set up service to change it - if rospy.has_param("~send_commands"): - send_commands = rospy.get_param("~send_commands") - rospy.Service("ros_control_bridge/send_commands", SetBool, set_send_commands) - rospy.loginfo(f"send_commands: {send_commands}") - - # Get (possible) 'time_from_start' parameter and set up subscriber to change it - if rospy.has_param("~time_from_start"): - time_from_start = rospy.get_param("~time_from_start") - rospy.loginfo(f"time_from_start: {time_from_start}") - rospy.Subscriber("ros_control_bridge/time_from_start", Float32, time_from_start_cb) + if rospy.has_param("~send_commands_"): + send_commands_ = rospy.get_param("~send_commands_") + rospy.Service("ros_control_bridge/send_commands_", SetBool, set_send_commands_) + rospy.loginfo(f"send_commands_: {send_commands_}") + + # Get (possible) 'time_from_start_' parameter and set up subscriber to change it + if rospy.has_param("~time_from_start_"): + time_from_start_ = rospy.get_param("~time_from_start_") + rospy.loginfo(f"time_from_start_: {time_from_start_}") + rospy.Subscriber( + "ros_control_bridge/time_from_start_", Float32, time_from_start__cb + ) # Set up subscriber to CartesI/O solution topic rospy.Subscriber("cartesian/solution", JointState, io_callback) @@ -214,6 +249,16 @@ def control_initiator_cb(msg): "/streamdeck/ros_control_bridge_initiator", Bool, control_initiator_cb ) + # Get (possible) 'wrist_force_limit' parameter and set up ft subscribers for emergency + if rospy.has_param("~wrist_force_limit"): + wrist_force_limit = rospy.get_param("~wrist_force_limit") + if rospy.has_param("~wrist_torque_limit"): + wrist_torque_limit = rospy.get_param("~wrist_torque_limit") + rospy.loginfo(f"wrist_force_limit: {wrist_force_limit} N") + rospy.loginfo(f"wrist_torque_limit: {wrist_torque_limit} Nm") + rospy.Subscriber("wrist_left_ft/corrected", WrenchStamped, left_ft_callback) + rospy.Subscriber("wrist_right_ft/corrected", WrenchStamped, right_ft_callback) + while not rospy.is_shutdown(): if exit_: break @@ -221,6 +266,3 @@ def control_initiator_cb(msg): rospy.loginfo("Exiting ros_control_bridge") os._exit(0) - - rospy.loginfo("Exiting ros_control_bridge") - os._exit(0) From 925f16c92c7299650a7b77afedd958b62f6d6b13 Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Fri, 11 Oct 2024 13:50:18 +0200 Subject: [PATCH 35/56] Fixed ft cb --- python/ros_control_bridge.py | 32 +++++++++++++++++++++++--------- 1 file changed, 23 insertions(+), 9 deletions(-) diff --git a/python/ros_control_bridge.py b/python/ros_control_bridge.py index c8d23cf..a7eccca 100755 --- a/python/ros_control_bridge.py +++ b/python/ros_control_bridge.py @@ -73,19 +73,19 @@ def left_ft_callback(data: WrenchStamped): global wrist_torque_limit global send_commands_ if ( - data.wrench.force.x > wrist_force_limit - or data.wrench.force.y > wrist_force_limit - or data.wrench.force.z > wrist_force_limit + abs(data.wrench.force.x) > wrist_force_limit + or abs(data.wrench.force.y) > wrist_force_limit + or abs(data.wrench.force.z) > wrist_force_limit ): - rospy.logwarn(f"Exceeded wrist force limit. Motion disabled") + rospy.logwarn(f"Exceeded left wrist force limit. Motion disabled") send_commands_ = False if ( - data.wrench.torque.x > wrist_torque_limit - or data.wrench.torque.y > wrist_torque_limit - or data.wrench.torque.z > wrist_torque_limit + abs(data.wrench.torque.x) > wrist_torque_limit + or abs(data.wrench.torque.y) > wrist_torque_limit + or abs(data.wrench.torque.z) > wrist_torque_limit ): - rospy.logwarn(f"Exceeded wrist torque limit. Motion disabled") + rospy.logwarn(f"Exceeded left wrist torque limit. Motion disabled") send_commands_ = False @@ -93,7 +93,21 @@ def right_ft_callback(data: WrenchStamped): global wrist_force_limit global wrist_torque_limit global send_commands_ - pass + if ( + abs(data.wrench.force.x) > wrist_force_limit + or abs(data.wrench.force.y) > wrist_force_limit + or abs(data.wrench.force.z) > wrist_force_limit + ): + rospy.logwarn(f"Exceeded right wrist force limit. Motion disabled") + send_commands_ = False + + if ( + abs(data.wrench.torque.x) > wrist_torque_limit + or abs(data.wrench.torque.y) > wrist_torque_limit + or abs(data.wrench.torque.z) > wrist_torque_limit + ): + rospy.logwarn(f"Exceeded right wrist torque limit. Motion disabled") + send_commands_ = False def io_callback(data: JointState): From 4b71251f316569d44b00c30f0ede294615a4a098 Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Fri, 11 Oct 2024 14:53:16 +0200 Subject: [PATCH 36/56] Minors --- python/ros_control_bridge.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/ros_control_bridge.py b/python/ros_control_bridge.py index a7eccca..04e3255 100755 --- a/python/ros_control_bridge.py +++ b/python/ros_control_bridge.py @@ -64,7 +64,7 @@ def fill_cms_msg(data: JointState, cmd_msg: JointTrajectory, send_velocity=True) cmd_msg.points[0].velocities[cmd_msg.joint_names.index(joint_name)] = ( data.velocity[data.name.index(joint_name)] ) - cmd_msg.points[0].time_from_start_ = rospy.Duration(time_from_start_) + cmd_msg.points[0].time_from_start = rospy.Duration(time_from_start_) return cmd_msg From 39ba23924fb2d47e430844e08a4cb1ac6d1f0fbb Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Fri, 11 Oct 2024 15:18:03 +0200 Subject: [PATCH 37/56] Minor formatting --- python/ros_control_bridge.py | 36 ++++++++++++++++++------------------ 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/python/ros_control_bridge.py b/python/ros_control_bridge.py index 04e3255..4120271 100755 --- a/python/ros_control_bridge.py +++ b/python/ros_control_bridge.py @@ -68,7 +68,7 @@ def fill_cms_msg(data: JointState, cmd_msg: JointTrajectory, send_velocity=True) return cmd_msg -def left_ft_callback(data: WrenchStamped): +def left_ft_cb(data: WrenchStamped): global wrist_force_limit global wrist_torque_limit global send_commands_ @@ -89,7 +89,7 @@ def left_ft_callback(data: WrenchStamped): send_commands_ = False -def right_ft_callback(data: WrenchStamped): +def right_ft_cb(data: WrenchStamped): global wrist_force_limit global wrist_torque_limit global send_commands_ @@ -158,25 +158,25 @@ def init_cmd_msg(cmd_msg: JointTrajectory, joint_names): return cmd_msg -def set_send_commands_(req: SetBool): +def set_send_commands_cb(req: SetBool): global send_commands_ send_commands_ = req.data response = SetBoolResponse() response.success = True - response.message = f"send_commands_: {req.data}" - rospy.loginfo(f"Setting 'send_commands_: {req.data}'") + response.message = f"send_commands: {req.data}" + rospy.loginfo(f"Setting 'send_commands: {req.data}'") return response -def time_from_start__cb(msg: Float32): +def set_time_from_start_cb(msg: Float32): global time_from_start_ if msg.data != time_from_start_: if msg.data >= 0.1: time_from_start_ = msg.data - rospy.loginfo(f"Setting 'time_from_start_: {time_from_start_}'") + rospy.loginfo(f"Setting 'time_from_start: {time_from_start_}'") else: rospy.logwarn( - "Trying to set a too low 'time_from_start_'. It must be >= 0.1secs" + "Trying to set a too low 'time_from_start'. It must be >= 0.1secs" ) @@ -242,17 +242,17 @@ def control_initiator_cb(msg): ) # Get (possible) enable/disable parameter and set up service to change it - if rospy.has_param("~send_commands_"): - send_commands_ = rospy.get_param("~send_commands_") - rospy.Service("ros_control_bridge/send_commands_", SetBool, set_send_commands_) + if rospy.has_param("~send_commands"): + send_commands_ = rospy.get_param("~send_commands") + rospy.Service("ros_control_bridge/send_commands", SetBool, set_send_commands_cb) rospy.loginfo(f"send_commands_: {send_commands_}") - # Get (possible) 'time_from_start_' parameter and set up subscriber to change it - if rospy.has_param("~time_from_start_"): - time_from_start_ = rospy.get_param("~time_from_start_") - rospy.loginfo(f"time_from_start_: {time_from_start_}") + # Get (possible) 'time_from_start' parameter and set up subscriber to change it + if rospy.has_param("~time_from_start"): + time_from_start_ = rospy.get_param("~time_from_start") + rospy.loginfo(f"time_from_start: {time_from_start_}") rospy.Subscriber( - "ros_control_bridge/time_from_start_", Float32, time_from_start__cb + "ros_control_bridge/time_from_start", Float32, set_time_from_start_cb ) # Set up subscriber to CartesI/O solution topic @@ -270,8 +270,8 @@ def control_initiator_cb(msg): wrist_torque_limit = rospy.get_param("~wrist_torque_limit") rospy.loginfo(f"wrist_force_limit: {wrist_force_limit} N") rospy.loginfo(f"wrist_torque_limit: {wrist_torque_limit} Nm") - rospy.Subscriber("wrist_left_ft/corrected", WrenchStamped, left_ft_callback) - rospy.Subscriber("wrist_right_ft/corrected", WrenchStamped, right_ft_callback) + rospy.Subscriber("wrist_left_ft/corrected", WrenchStamped, left_ft_cb) + rospy.Subscriber("wrist_right_ft/corrected", WrenchStamped, right_ft_cb) while not rospy.is_shutdown(): if exit_: From 0e7e8c99ed7b7674491c73ba08f70017602402b3 Mon Sep 17 00:00:00 2001 From: Clemente Donoso Krauss <49209768+CDonosoK@users.noreply.github.com> Date: Tue, 15 Oct 2024 13:24:41 +0200 Subject: [PATCH 38/56] Update teleop_gripper_bridge.py Comment line: gripper_cmd_msg.header.stamp = rospy.get_rostime() Now the teleoperation is workin --- python/teleop_gripper_bridge.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/teleop_gripper_bridge.py b/python/teleop_gripper_bridge.py index 781fb1a..4ae9aa3 100755 --- a/python/teleop_gripper_bridge.py +++ b/python/teleop_gripper_bridge.py @@ -35,7 +35,7 @@ def io_callback(data: Float32): ] = gripper gripper_cmd_msg.points[0].time_from_start = rospy.Duration(time_duration) - gripper_cmd_msg.header.stamp = rospy.get_rostime() + #gripper_cmd_msg.header.stamp = rospy.get_rostime() gripper_cmd_pub.publish(gripper_cmd_msg) From badc94a0ec7b80590466bb173672a8d124fff132 Mon Sep 17 00:00:00 2001 From: Clemente Donoso Krauss <49209768+CDonosoK@users.noreply.github.com> Date: Tue, 15 Oct 2024 13:26:26 +0200 Subject: [PATCH 39/56] rosnode is not anymore anonymous Change the node param "anonymous" to False to kill it remotely --- python/ros_control_bridge.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/ros_control_bridge.py b/python/ros_control_bridge.py index 4120271..b0b5c0b 100755 --- a/python/ros_control_bridge.py +++ b/python/ros_control_bridge.py @@ -187,7 +187,7 @@ def control_initiator_cb(msg): if __name__ == "__main__": - rospy.init_node("ros_control_bridge", anonymous=True) + rospy.init_node("ros_control_bridge", anonymous=False) rospy.loginfo(f"SEND_VELOCITY: {SEND_VELOCITY}") # Wait to receive the first msg from /joint_states From 7445b8b67145e1c93f623ee37a718d6308536964 Mon Sep 17 00:00:00 2001 From: Clemente Donoso Krauss <49209768+CDonosoK@users.noreply.github.com> Date: Tue, 15 Oct 2024 13:29:04 +0200 Subject: [PATCH 40/56] rosnode is not anymore anonymous Beta Change the node param "anonymous" to False to kill it remotely --- python/teleop_bridge.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/teleop_bridge.py b/python/teleop_bridge.py index 031fe07..d954fea 100755 --- a/python/teleop_bridge.py +++ b/python/teleop_bridge.py @@ -65,7 +65,7 @@ def callback(data: PoseStamped): if __name__ == "__main__": - rospy.init_node("teleop_bridge", anonymous=True) + rospy.init_node("teleop_bridge", anonymous=False) if rospy.has_param("~controlled_frame"): controlled_frame = rospy.get_param("~controlled_frame") From da9a1e94aeda21d8e1c6597033c2508b6df5145c Mon Sep 17 00:00:00 2001 From: Clemente Donoso Date: Tue, 15 Oct 2024 15:08:38 +0200 Subject: [PATCH 41/56] Update gitignore file and change values of the force and torque limit, still need to tune this values --- .gitignore | 62 ++++++++++++++++++++++++++++++++++++ python/ros_control_bridge.py | 6 ++-- 2 files changed, 66 insertions(+), 2 deletions(-) diff --git a/.gitignore b/.gitignore index 05676d1..a48283b 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,64 @@ *.npy *.csv + +devel/ +logs/ +build/ +bin/ +lib/ +msg_gen/ +srv_gen/ +msg/*Action.msg +msg/*ActionFeedback.msg +msg/*ActionGoal.msg +msg/*ActionResult.msg +msg/*Feedback.msg +msg/*Goal.msg +msg/*Result.msg +msg/_*.py +build_isolated/ +devel_isolated/ + +# Generated by dynamic reconfigure +*.cfgc +/cfg/cpp/ +/cfg/*.py + +# Ignore generated docs +*.dox +*.wikidoc + +# eclipse stuff +.project +.cproject + +# qcreator stuff +CMakeLists.txt.user + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +/planning/cfg +/planning/docs +/planning/src + +*~ + +# Emacs +.#* + +# Vscode +.vscode +/log + +# Catkin custom files +CATKIN_IGNORE +hardware/simpleGimball/~$handle.SLDPRT +hardware/simpleGimball/~$hub1.SLDPRT +hardware/simpleGimball/~$motorgl30.SLDPRT +hardware/simpleGimball/~$motorMount1.SLDPRT +hardware/simpleGimball/~$motorMount2.SLDPRT +hardware/~$armAssembly.SLDASM diff --git a/python/ros_control_bridge.py b/python/ros_control_bridge.py index b0b5c0b..20bb245 100755 --- a/python/ros_control_bridge.py +++ b/python/ros_control_bridge.py @@ -25,8 +25,10 @@ time_from_start_ = 0.2 -wrist_force_limit = 20 -wrist_torque_limit = 2 +# max value for each axis: x = -42, y = -58, z = -69 +wrist_force_limit = 71.0 +# max value for each axis: x = 8, y = -11, z = 3.0 +wrist_torque_limit = 12.0 exit_ = False From e392234f27d83e72d3b56398239a27d8c16ad466 Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Tue, 15 Oct 2024 18:31:38 +0200 Subject: [PATCH 42/56] Test current limit setter --- python/ros_control_bridge.py | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/python/ros_control_bridge.py b/python/ros_control_bridge.py index 20bb245..0131787 100755 --- a/python/ros_control_bridge.py +++ b/python/ros_control_bridge.py @@ -7,6 +7,7 @@ from geometry_msgs.msg import Twist, WrenchStamped from std_msgs.msg import Float32, Bool from std_srvs.srv import SetBool, SetBoolResponse +from pal_control_msgs.msg import ActuatorCurrentLimit head_cmd_pub_ = None head_cmd_msg_ = JointTrajectory() @@ -196,6 +197,26 @@ def control_initiator_cb(msg): data = rospy.wait_for_message("joint_states", JointState, timeout=5) set_initial_configuration(data) + ### TEST + curr_limit_pub = rospy.Publisher( + "/gripper_left_current_limit_controller/command", + ActuatorCurrentLimit, + queue_size=1, + ) + curr_lim_msg = ActuatorCurrentLimit() + curr_lim_msg.actuator_names = [ + "gripper_left_left_finger_motor", + "gripper_left_right_finger_motor", + ] + curr_lim_msg.current_limits = [0.5, 0.6] + while curr_limit_pub.get_num_connections() < 1: + # wait for a connection to publisher + rospy.loginfo("Connecting to current limit controller...") + pass + + curr_limit_pub.publish(curr_lim_msg) + # ... + # Set up command publishers and msgs head_cmd_pub_ = rospy.Publisher( "head_controller/command", JointTrajectory, queue_size=1 From 1034aa6b7c0afe2c690bd0ba6da0f2b95b6aa882 Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Tue, 15 Oct 2024 18:49:15 +0200 Subject: [PATCH 43/56] Added initial lowering of grippers' current limits --- python/ros_control_bridge.py | 31 ++++++++++++++++++++++--------- 1 file changed, 22 insertions(+), 9 deletions(-) diff --git a/python/ros_control_bridge.py b/python/ros_control_bridge.py index 0131787..cf9ded2 100755 --- a/python/ros_control_bridge.py +++ b/python/ros_control_bridge.py @@ -197,25 +197,38 @@ def control_initiator_cb(msg): data = rospy.wait_for_message("joint_states", JointState, timeout=5) set_initial_configuration(data) - ### TEST - curr_limit_pub = rospy.Publisher( + # Lower maximum currents for the grippers + lg_curr_lim_pub = rospy.Publisher( "/gripper_left_current_limit_controller/command", ActuatorCurrentLimit, queue_size=1, ) - curr_lim_msg = ActuatorCurrentLimit() - curr_lim_msg.actuator_names = [ + lg_curr_lim_msg = ActuatorCurrentLimit() + lg_curr_lim_msg.actuator_names = [ "gripper_left_left_finger_motor", "gripper_left_right_finger_motor", ] - curr_lim_msg.current_limits = [0.5, 0.6] - while curr_limit_pub.get_num_connections() < 1: + lg_curr_lim_msg.current_limits = [0.75, 0.75] + while lg_curr_lim_pub.get_num_connections() < 1: # wait for a connection to publisher - rospy.loginfo("Connecting to current limit controller...") pass + lg_curr_lim_pub.publish(lg_curr_lim_msg) - curr_limit_pub.publish(curr_lim_msg) - # ... + rg_curr_lim_pub = rospy.Publisher( + "/gripper_right_current_limit_controller/command", + ActuatorCurrentLimit, + queue_size=1, + ) + rg_curr_lim_msg = ActuatorCurrentLimit() + rg_curr_lim_msg.actuator_names = [ + "gripper_right_left_finger_motor", + "gripper_right_right_finger_motor", + ] + rg_curr_lim_msg.current_limits = [0.75, 0.75] + while rg_curr_lim_pub.get_num_connections() < 1: + # wait for a connection to publisher + pass + rg_curr_lim_pub.publish(rg_curr_lim_msg) # Set up command publishers and msgs head_cmd_pub_ = rospy.Publisher( From 6cee5d0d26bd74d9dcf116c29ed589455009dad8 Mon Sep 17 00:00:00 2001 From: Clemente Donoso Date: Thu, 17 Oct 2024 15:32:13 +0200 Subject: [PATCH 44/56] Minor changes to the gripper right min value, to close it more. And comment the current control --- launch/teleop.launch | 2 +- python/ros_control_bridge.py | 62 ++++++++++++++++++------------------ 2 files changed, 32 insertions(+), 32 deletions(-) diff --git a/launch/teleop.launch b/launch/teleop.launch index 653b340..508f11e 100644 --- a/launch/teleop.launch +++ b/launch/teleop.launch @@ -36,7 +36,7 @@ - + diff --git a/python/ros_control_bridge.py b/python/ros_control_bridge.py index cf9ded2..1bcb0d5 100755 --- a/python/ros_control_bridge.py +++ b/python/ros_control_bridge.py @@ -198,37 +198,37 @@ def control_initiator_cb(msg): set_initial_configuration(data) # Lower maximum currents for the grippers - lg_curr_lim_pub = rospy.Publisher( - "/gripper_left_current_limit_controller/command", - ActuatorCurrentLimit, - queue_size=1, - ) - lg_curr_lim_msg = ActuatorCurrentLimit() - lg_curr_lim_msg.actuator_names = [ - "gripper_left_left_finger_motor", - "gripper_left_right_finger_motor", - ] - lg_curr_lim_msg.current_limits = [0.75, 0.75] - while lg_curr_lim_pub.get_num_connections() < 1: - # wait for a connection to publisher - pass - lg_curr_lim_pub.publish(lg_curr_lim_msg) - - rg_curr_lim_pub = rospy.Publisher( - "/gripper_right_current_limit_controller/command", - ActuatorCurrentLimit, - queue_size=1, - ) - rg_curr_lim_msg = ActuatorCurrentLimit() - rg_curr_lim_msg.actuator_names = [ - "gripper_right_left_finger_motor", - "gripper_right_right_finger_motor", - ] - rg_curr_lim_msg.current_limits = [0.75, 0.75] - while rg_curr_lim_pub.get_num_connections() < 1: - # wait for a connection to publisher - pass - rg_curr_lim_pub.publish(rg_curr_lim_msg) + # lg_curr_lim_pub = rospy.Publisher( + # "/gripper_left_current_limit_controller/command", + # ActuatorCurrentLimit, + # queue_size=1, + # ) + # lg_curr_lim_msg = ActuatorCurrentLimit() + # lg_curr_lim_msg.actuator_names = [ + # "gripper_left_left_finger_motor", + # "gripper_left_right_finger_motor", + # ] + # lg_curr_lim_msg.current_limits = [0.75, 0.75] + # while lg_curr_lim_pub.get_num_connections() < 1: + # # wait for a connection to publisher + # pass + # lg_curr_lim_pub.publish(lg_curr_lim_msg) + + # rg_curr_lim_pub = rospy.Publisher( + # "/gripper_right_current_limit_controller/command", + # ActuatorCurrentLimit, + # queue_size=1, + # ) + # rg_curr_lim_msg = ActuatorCurrentLimit() + # rg_curr_lim_msg.actuator_names = [ + # "gripper_right_left_finger_motor", + # "gripper_right_right_finger_motor", + # ] + # rg_curr_lim_msg.current_limits = [0.75, 0.75] + # while rg_curr_lim_pub.get_num_connections() < 1: + # # wait for a connection to publisher + # pass + # rg_curr_lim_pub.publish(rg_curr_lim_msg) # Set up command publishers and msgs head_cmd_pub_ = rospy.Publisher( From 7b79e35fab37d7f1907142befd9f968ff60e4e3f Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Fri, 18 Oct 2024 19:36:56 +0200 Subject: [PATCH 45/56] Demo recorder interactive terminal menu TODO: call to orbbec log cmd topic --- launch/demo_recorder.launch | 2 +- python/demo/postprocess_demo.py | 196 ++++++++++- python/demo_recorder.py | 598 +++++++++++++++++++++----------- 3 files changed, 589 insertions(+), 207 deletions(-) diff --git a/launch/demo_recorder.launch b/launch/demo_recorder.launch index b3ffc57..942ff04 100644 --- a/launch/demo_recorder.launch +++ b/launch/demo_recorder.launch @@ -1,7 +1,7 @@ "/home/forest_ws/src/tiago_dual_cartesio_config/python/demo" + ["orbbec_head"] [0] - 100 diff --git a/python/demo/postprocess_demo.py b/python/demo/postprocess_demo.py index 6ff8731..576f5d4 100644 --- a/python/demo/postprocess_demo.py +++ b/python/demo/postprocess_demo.py @@ -4,20 +4,63 @@ import numpy as np import os -subf = "" +joint_names = [ + "arm_left_1_joint", + "arm_left_2_joint", + "arm_left_3_joint", + "arm_left_4_joint", + "arm_left_5_joint", + "arm_left_6_joint", + "arm_left_7_joint", + "arm_right_1_joint", + "arm_right_2_joint", + "arm_right_3_joint", + "arm_right_4_joint", + "arm_right_5_joint", + "arm_right_6_joint", + "arm_right_7_joint", + "gripper_left_left_finger_joint", + "gripper_left_right_finger_joint", + "gripper_right_left_finger_joint", + "gripper_right_right_finger_joint", + "head_1_joint", + "head_2_joint", + "torso_lift_joint", + "wheel_front_left_joint", + "wheel_front_right_joint", + "wheel_rear_left_joint", + "wheel_rear_right_joint", +] +subf = "dummy_log" + +joints = np.load(os.path.join(subf, "data_joints.npy"))[1:, :] +print("joints.shape", joints.shape) + actual_left_pose = np.load(os.path.join(subf, "data_actual_left_pose.npy"))[1:, :] +print("actual_left_pose.shape", actual_left_pose.shape) actual_right_pose = np.load(os.path.join(subf, "data_actual_right_pose.npy"))[1:, :] +print("actual_right_pose.shape", actual_right_pose.shape) + target_left_pose = np.load(os.path.join(subf, "data_target_left_pose.npy"))[1:, :] +print("target_left_pose.shape", target_left_pose.shape) target_right_pose = np.load(os.path.join(subf, "data_target_right_pose.npy"))[1:, :] -right_gripper = np.load(os.path.join(subf, "data_right_gripper.npy"))[1:, :] -left_gripper = np.load(os.path.join(subf, "data_left_gripper.npy"))[1:, :] +print("target_right_pose.shape", target_right_pose.shape) + pose_tag_0 = np.load(os.path.join(subf, "data_pose_tag_0.npy"))[1:, :] +print("pose_tag_0.shape", pose_tag_0.shape) fig = plt.figure() fig.suptitle("Left EE", fontsize=16) +plt.plot( + actual_left_pose[:, -1] - actual_left_pose[0, -1], + actual_left_pose[:, 0], + color="tab:red", + linewidth=2, + label="true x", +) plt.plot( target_left_pose[:, -1] - target_left_pose[0, -1], target_left_pose[:, 0], @@ -26,6 +69,13 @@ linewidth=2, label="target x", ) +plt.plot( + actual_left_pose[:, -1] - actual_left_pose[0, -1], + actual_left_pose[:, 1], + color="tab:green", + linewidth=2, + label="true y", +) plt.plot( target_left_pose[:, -1] - target_left_pose[0, -1], target_left_pose[:, 1], @@ -34,6 +84,13 @@ linewidth=2, label="target y", ) +plt.plot( + actual_left_pose[:, -1] - actual_left_pose[0, -1], + actual_left_pose[:, 2], + color="tab:blue", + linewidth=2, + label="true z", +) plt.plot( target_left_pose[:, -1] - target_left_pose[0, -1], target_left_pose[:, 2], @@ -42,12 +99,21 @@ linewidth=2, label="target z", ) +plt.xlabel("Time [s]") +plt.ylabel("Position [m]") plt.legend() plt.grid() plt.show() fig = plt.figure() fig.suptitle("Right EE", fontsize=16) +plt.plot( + actual_right_pose[:, -1] - actual_right_pose[0, -1], + actual_right_pose[:, 0], + color="tab:red", + linewidth=2, + label="true x", +) plt.plot( target_right_pose[:, -1] - target_right_pose[0, -1], target_right_pose[:, 0], @@ -56,6 +122,13 @@ linewidth=2, label="target x", ) +plt.plot( + actual_right_pose[:, -1] - actual_right_pose[0, -1], + actual_right_pose[:, 1], + color="tab:green", + linewidth=2, + label="true y", +) plt.plot( target_right_pose[:, -1] - target_right_pose[0, -1], target_right_pose[:, 1], @@ -64,6 +137,13 @@ linewidth=2, label="target y", ) +plt.plot( + actual_right_pose[:, -1] - actual_right_pose[0, -1], + actual_right_pose[:, 2], + color="tab:blue", + linewidth=2, + label="true z", +) plt.plot( target_right_pose[:, -1] - target_right_pose[0, -1], target_right_pose[:, 2], @@ -72,6 +152,116 @@ linewidth=2, label="target z", ) +plt.xlabel("Time [s]") +plt.ylabel("Position [m]") +plt.legend() +plt.grid() +plt.show() + +fig = plt.figure() +fig.suptitle("Pose Tag 0", fontsize=16) +plt.plot( + pose_tag_0[:, -1] - pose_tag_0[0, -1], + pose_tag_0[:, 0], + color="tab:red", + linewidth=2, + label="x", +) +plt.plot( + pose_tag_0[:, -1] - pose_tag_0[0, -1], + pose_tag_0[:, 1], + color="tab:green", + linewidth=2, + label="y", +) +plt.plot( + pose_tag_0[:, -1] - pose_tag_0[0, -1], + pose_tag_0[:, 2], + color="tab:blue", + linewidth=2, + label="z", +) +plt.xlabel("Time [s]") +plt.ylabel("Position [m]") +plt.legend() +plt.grid() +plt.show() + + +fig = plt.figure() +fig.suptitle("Left Gripper", fontsize=16) +plt.plot( + joints[:, -1] - joints[0, -1], + joints[:, 14], + color="tab:red", + linewidth=2, + label="left finger joint", +) +plt.plot( + joints[:, -1] - joints[0, -1], + joints[:, 15], + color="tab:green", + linewidth=2, + label="right finger joint", +) +plt.xlabel("Time [s]") +plt.ylabel("Position [m]") +plt.legend() +plt.grid() +plt.show() + +fig = plt.figure() +fig.suptitle("Right Gripper", fontsize=16) +plt.plot( + joints[:, -1] - joints[0, -1], + joints[:, 16], + color="tab:red", + linewidth=2, + label="left finger joint", +) +plt.plot( + joints[:, -1] - joints[0, -1], + joints[:, 17], + color="tab:green", + linewidth=2, + label="right finger joint", +) +plt.xlabel("Time [s]") +plt.ylabel("Position [m]") +plt.legend() +plt.grid() +plt.show() + +fig = plt.figure() +fig.suptitle("Left arm joints", fontsize=16) +plt.xlabel("Time [s]") +start_idx = 0 +for i in range(7): + plt.plot( + joints[:, -1] - joints[0, -1], + joints[:, start_idx + i], + linewidth=2, + label=joint_names[start_idx + i], + ) +plt.xlabel("Time [s]") +plt.ylabel("Angle [rad]") +plt.legend() +plt.grid() +plt.show() + +fig = plt.figure() +fig.suptitle("Right arm joints", fontsize=16) +plt.xlabel("Time [s]") +start_idx = 7 +for i in range(7): + plt.plot( + joints[:, -1] - joints[0, -1], + joints[:, start_idx + i], + linewidth=2, + label=joint_names[start_idx + i], + ) +plt.xlabel("Time [s]") +plt.ylabel("Angle [rad]") plt.legend() plt.grid() plt.show() diff --git a/python/demo_recorder.py b/python/demo_recorder.py index bcb4e9d..696aeeb 100755 --- a/python/demo_recorder.py +++ b/python/demo_recorder.py @@ -1,279 +1,471 @@ #!/usr/bin/env python3 - +from colorama import Fore, Style from datetime import datetime +import glob import numpy as np -import threading import os +from select import select +import sys +import threading + +if sys.platform == "win32": + import msvcrt +else: + import termios + import tty import rospy import tf2_ros - from sensor_msgs.msg import JointState +from std_msgs.msg import Float32 from trajectory_msgs.msg import JointTrajectory from geometry_msgs.msg import PoseStamped class DemoRecorder: - def __init__(self): - rospy.init_node("demo_recorder") + def __init__(self, folder_path=os.environ["HOME"], cameras=[], tags=[]): - # Load params - if rospy.has_param("~tags"): - tags = rospy.get_param("~tags") - else: - tags = [] - if rospy.has_param("~folder_path"): - folder_path = rospy.get_param("~folder_path") - else: - folder_path = os.environ["HOME"] - if rospy.has_param("~tf_hz"): - self.tf_hz = rospy.get_param("~tf_hz") - else: - self.tf_hz = 100 - - now = datetime.now() # current date and time - date_time = now.strftime("%m-%d-%Y_%H-%M-%S") - self.save_folder = os.path.join(folder_path, date_time) - os.mkdir(self.save_folder) - - # Set dump file on node shutdown - rospy.on_shutdown(self.save_data) + # Init member variables + self.folder_path = folder_path + self.cameras = cameras + self.tags = tags + self.demo_idx = 0 + self.flg_record = False # Set up tf2 listener self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) + # Select new initial demo index + subfolders = [f.path for f in os.scandir(self.folder_path) if f.is_dir()] + for subf in subfolders: + try: + i = int( + subf[ + subf.startswith(os.path.join(self.folder_path, "")) + and len(os.path.join(self.folder_path, "")) : + ] + ) + if i >= self.demo_idx: + self.demo_idx = i + 1 + except: + pass + self.save_folder = os.path.join(self.folder_path, str(self.demo_idx)) + os.mkdir(self.save_folder) + + self.init_log_variables() + self.init_subscribers() + # Wait for tf listener rospy.sleep(1) - # Init ee pose recorder - self.data_actual_left_pose = np.zeros((1, 8)) - self.data_actual_right_pose = np.zeros((1, 8)) - self.data_target_left_pose = np.zeros((1, 8)) - self.data_target_right_pose = np.zeros((1, 8)) + print("Demo recorder initialized") + print(f"Demo index = {self.demo_idx}") + def init_log_variables(self): # Init joint state recorder self.data_joints = np.zeros((1, 26)) + + # Init recorder(s) for pose tags + self.data_tags = {} + for camera in self.cameras: + self.data_tags[camera] = {} + for tag in self.tags: + self.data_tags[camera][int(tag)] = np.zeros((1, 8)) + + # Init ee pose recorder + self.data_measured_left_pose = np.zeros((1, 8)) + self.data_cartesio_left_pose = np.zeros((1, 8)) + self.data_teleop_left_pose = np.zeros((1, 8)) + self.data_teleop_right_pose = np.zeros((1, 8)) + self.data_cartesio_right_pose = np.zeros((1, 8)) + self.data_measured_right_pose = np.zeros((1, 8)) + + # Init gripper commands recorder + self.data_left_gripper_cmd = np.zeros((1, 2)) + self.data_right_gripper_cmd = np.zeros((1, 2)) + + def init_subscribers(self): + # Init joint state recorder self.sub_joint_state = rospy.Subscriber( "/joint_states", JointState, self.cb_joint_state ) - # Init left/right gripper recorder - self.data_left_gripper = np.zeros((1, 3)) - self.data_right_gripper = np.zeros((1, 3)) - self.sub_left_gripper = rospy.Subscriber( - "/gripper_left_controller/command", JointTrajectory, self.cb_left_gripper + # Init recorder(s) for pose tags + for camera in self.cameras: + for tag in self.tags: + rospy.Subscriber( + f"/inria_orbbec_tags/pose_tag_{tag}", + PoseStamped, + self.cb_tag, + (camera, int(tag)), + ) + + # Init ee pose recorder + self.sub_left_teleop = rospy.Subscriber( + "/dxl_input/pos_left", PoseStamped, self.cb_left_teleop ) - self.sub_right_gripper = rospy.Subscriber( - "/gripper_right_controller/command", JointTrajectory, self.cb_right_gripper + + self.sub_right_teleop = rospy.Subscriber( + "/dxl_input/pos_right", PoseStamped, self.cb_right_teleop ) - # Init recorder(s) for april tags - self.data_tags = {} - for tag in tags: - self.data_tags[int(tag)] = np.zeros((1, 8)) - rospy.Subscriber( - f"/inria_orbbec_tags/pose_tag_{tag}", - PoseStamped, - self.cb_tag, - (int(tag)), - ) + # Init gripper commands recorder + self.sub_left_gripper_cmd = rospy.Subscriber( + "/dxl_input/gripper_left", Float32, self.cb_left_gripper + ) - rospy.loginfo(f"Demo recorder initialized") + self.sub_right_gripper_cmd = rospy.Subscriber( + "/dxl_input/gripper_right", Float32, self.cb_right_gripper + ) def cb_joint_state(self, msg: JointState): """Store joints state data (format: nPoints x [j1, ..., j25, time_stamp])""" - new_joints = np.zeros((1, 26)) - for i in range(25): - new_joints[0, i] = msg.position[i] - new_joints[0, -1] = msg.header.stamp.secs + 10**-9 * msg.header.stamp.nsecs - self.data_joints = np.append(self.data_joints, new_joints, 0) - - def cb_left_gripper(self, msg: JointTrajectory): - """Store left gripper data (format: nPoints x [joint_left, joint_right, time_stamp])""" - new_left_gripper = np.zeros((1, 3)) - new_left_gripper[0, 0] = msg.points[0].positions[ - msg.joint_names.index("gripper_left_left_finger_joint") - ] - new_left_gripper[0, 1] = msg.points[0].positions[ - msg.joint_names.index("gripper_left_right_finger_joint") - ] - new_left_gripper[0, 2] = msg.header.stamp.secs + 10**-9 * msg.header.stamp.nsecs - self.data_left_gripper = np.append(self.data_left_gripper, new_left_gripper, 0) - - def cb_right_gripper(self, msg: JointTrajectory): - """Store right gripper data (format: nPoints x [joint_left, joint_right, time_stamp])""" - new_right_gripper = np.zeros((1, 3)) - new_right_gripper[0, 0] = msg.points[0].positions[ - msg.joint_names.index("gripper_right_left_finger_joint") - ] - new_right_gripper[0, 1] = msg.points[0].positions[ - msg.joint_names.index("gripper_right_right_finger_joint") - ] - new_right_gripper[0, 2] = ( - msg.header.stamp.secs + 10**-9 * msg.header.stamp.nsecs - ) - self.data_right_gripper = np.append( - self.data_right_gripper, new_right_gripper, 0 - ) + if self.flg_record: + new_joints = np.zeros((1, 26)) + for i in range(25): + new_joints[0, i] = msg.position[i] + new_joints[0, -1] = int(datetime.now().timestamp() * 1000) # [ms] + self.data_joints = np.append(self.data_joints, new_joints, 0) - def cb_tag(self, msg: PoseStamped, args: int): + def cb_tag(self, msg: PoseStamped, args): """Store tags data (format: nTagsx [nPoints x [x, y, z, qx, qy, qz, qw, time_stamp]])""" - tag_id = args - new_tag = np.zeros((1, 8)) - new_tag[0, 0] = msg.pose.position.x - new_tag[0, 1] = msg.pose.position.y - new_tag[0, 2] = msg.pose.position.z - new_tag[0, 3] = msg.pose.position.x - new_tag[0, 4] = msg.pose.orientation.y - new_tag[0, 5] = msg.pose.orientation.z - new_tag[0, 6] = msg.pose.orientation.w - new_tag[0, 7] = msg.header.stamp.secs + 10**-9 * msg.header.stamp.nsecs - self.data_tags[tag_id] = np.append(self.data_tags[tag_id], new_tag, 0) - - def record(self): - """Query tf tree and store target and actual ee poses (format: nPoints x [x, y, z, qx, qy, qz, qw, time_stamp])""" - rate = rospy.Rate(self.tf_hz) - while not rospy.is_shutdown(): + if self.flg_record: + camera_name = args[0] + tag_id = args[1] + new_tag = np.zeros((1, 8)) + new_tag[0, 0] = msg.pose.position.x + new_tag[0, 1] = msg.pose.position.y + new_tag[0, 2] = msg.pose.position.z + new_tag[0, 3] = msg.pose.orientation.x + new_tag[0, 4] = msg.pose.orientation.y + new_tag[0, 5] = msg.pose.orientation.z + new_tag[0, 6] = msg.pose.orientation.w + new_tag[0, 7] = int( + msg.header.stamp.secs * 1000 + msg.header.stamp.nsecs * 10**-6 + ) # [ms] + self.data_tags[camera_name][tag_id] = np.append( + self.data_tags[camera_name][tag_id], new_tag, 0 + ) + + def cb_left_teleop(self, msg: PoseStamped): + """Store teleop pose, cartesio and measured ee poses (format: nPoints x [x, y, z, qx, qy, qz, qw, time_stamp])""" + if self.flg_record: try: - # base-to-left-ee (actual) + timestamp = int(datetime.now().timestamp() * 1000) + + # base-to-left-ee (teleop) + new_teleop_left_pose = np.zeros((1, 8)) + new_teleop_left_pose[0, 0] = msg.pose.position.x + new_teleop_left_pose[0, 1] = msg.pose.position.y + new_teleop_left_pose[0, 2] = msg.pose.position.z + new_teleop_left_pose[0, 3] = msg.pose.orientation.x + new_teleop_left_pose[0, 4] = msg.pose.orientation.y + new_teleop_left_pose[0, 5] = msg.pose.orientation.z + new_teleop_left_pose[0, 6] = msg.pose.orientation.w + new_teleop_left_pose[0, 7] = timestamp + self.data_teleop_left_pose = np.append( + self.data_teleop_left_pose, new_teleop_left_pose, 0 + ) + + # base-to-left-ee (measured) t = self.tf_buffer.lookup_transform( "base_link", "gripper_left_grasping_frame", rospy.Time(), ) - new_actual_left_pose = np.zeros((1, 8)) - new_actual_left_pose[0, 0] = t.transform.translation.x - new_actual_left_pose[0, 1] = t.transform.translation.y - new_actual_left_pose[0, 2] = t.transform.translation.z - new_actual_left_pose[0, 3] = t.transform.rotation.x - new_actual_left_pose[0, 4] = t.transform.rotation.y - new_actual_left_pose[0, 5] = t.transform.rotation.z - new_actual_left_pose[0, 6] = t.transform.rotation.w - new_actual_left_pose[0, 7] = ( - t.header.stamp.secs + 10**-9 * t.header.stamp.nsecs - ) - self.data_actual_left_pose = np.append( - self.data_actual_left_pose, new_actual_left_pose, 0 + new_measured_left_pose = np.zeros((1, 8)) + new_measured_left_pose[0, 0] = t.transform.translation.x + new_measured_left_pose[0, 1] = t.transform.translation.y + new_measured_left_pose[0, 2] = t.transform.translation.z + new_measured_left_pose[0, 3] = t.transform.rotation.x + new_measured_left_pose[0, 4] = t.transform.rotation.y + new_measured_left_pose[0, 5] = t.transform.rotation.z + new_measured_left_pose[0, 6] = t.transform.rotation.w + new_measured_left_pose[0, 7] = timestamp + self.data_measured_left_pose = np.append( + self.data_measured_left_pose, new_measured_left_pose, 0 ) - except Exception as err: - rospy.logwarn(err) - try: - # base-to-right-ee (actual) + # base-to-left-ee (cartesio) t = self.tf_buffer.lookup_transform( - "base_link", - "gripper_right_grasping_frame", + "ci/base_link", + "ci/gripper_left_grasping_frame", rospy.Time(), ) - new_actual_right_pose = np.zeros((1, 8)) - new_actual_right_pose[0, 0] = t.transform.translation.x - new_actual_right_pose[0, 1] = t.transform.translation.y - new_actual_right_pose[0, 2] = t.transform.translation.z - new_actual_right_pose[0, 3] = t.transform.rotation.x - new_actual_right_pose[0, 4] = t.transform.rotation.y - new_actual_right_pose[0, 5] = t.transform.rotation.z - new_actual_right_pose[0, 6] = t.transform.rotation.w - new_actual_right_pose[0, 7] = ( - t.header.stamp.secs + 10**-9 * t.header.stamp.nsecs - ) - self.data_actual_right_pose = np.append( - self.data_actual_right_pose, new_actual_right_pose, 0 + new_cartesio_left_pose = np.zeros((1, 8)) + new_cartesio_left_pose[0, 0] = t.transform.translation.x + new_cartesio_left_pose[0, 1] = t.transform.translation.y + new_cartesio_left_pose[0, 2] = t.transform.translation.z + new_cartesio_left_pose[0, 3] = t.transform.rotation.x + new_cartesio_left_pose[0, 4] = t.transform.rotation.y + new_cartesio_left_pose[0, 5] = t.transform.rotation.z + new_cartesio_left_pose[0, 6] = t.transform.rotation.w + new_cartesio_left_pose[0, 7] = timestamp + self.data_cartesio_left_pose = np.append( + self.data_cartesio_left_pose, new_cartesio_left_pose, 0 ) except Exception as err: - rospy.logwarn(err) + print(Fore.RED + err) + print(Style.RESET_ALL) + def cb_right_teleop(self, msg: PoseStamped): + """Store teleop pose, cartesio and measured ee poses (format: nPoints x [x, y, z, qx, qy, qz, qw, time_stamp])""" + if self.flg_record: try: - # base-to-left-ee (target) + timestamp = int(datetime.now().timestamp() * 1000) + + # base-to-right-ee (teleop) + new_teleop_right_pose = np.zeros((1, 8)) + new_teleop_right_pose[0, 0] = msg.pose.position.x + new_teleop_right_pose[0, 1] = msg.pose.position.y + new_teleop_right_pose[0, 2] = msg.pose.position.z + new_teleop_right_pose[0, 3] = msg.pose.orientation.x + new_teleop_right_pose[0, 4] = msg.pose.orientation.y + new_teleop_right_pose[0, 5] = msg.pose.orientation.z + new_teleop_right_pose[0, 6] = msg.pose.orientation.w + new_teleop_right_pose[0, 7] = timestamp + self.data_teleop_right_pose = np.append( + self.data_teleop_right_pose, new_teleop_right_pose, 0 + ) + + # base-to-right-ee (measured) t = self.tf_buffer.lookup_transform( - "ci/base_link", - "ci/gripper_left_grasping_frame", + "base_link", + "gripper_right_grasping_frame", rospy.Time(), ) - new_target_left_pose = np.zeros((1, 8)) - new_target_left_pose[0, 0] = t.transform.translation.x - new_target_left_pose[0, 1] = t.transform.translation.y - new_target_left_pose[0, 2] = t.transform.translation.z - new_target_left_pose[0, 3] = t.transform.rotation.x - new_target_left_pose[0, 4] = t.transform.rotation.y - new_target_left_pose[0, 5] = t.transform.rotation.z - new_target_left_pose[0, 6] = t.transform.rotation.w - new_target_left_pose[0, 7] = ( - t.header.stamp.secs + 10**-9 * t.header.stamp.nsecs + new_measured_right_pose = np.zeros((1, 8)) + new_measured_right_pose[0, 0] = t.transform.translation.x + new_measured_right_pose[0, 1] = t.transform.translation.y + new_measured_right_pose[0, 2] = t.transform.translation.z + new_measured_right_pose[0, 3] = t.transform.rotation.x + new_measured_right_pose[0, 4] = t.transform.rotation.y + new_measured_right_pose[0, 5] = t.transform.rotation.z + new_measured_right_pose[0, 6] = t.transform.rotation.w + new_measured_right_pose[0, 7] = timestamp + self.data_measured_right_pose = np.append( + self.data_measured_right_pose, new_measured_right_pose, 0 ) - self.data_target_left_pose = np.append( - self.data_target_left_pose, new_target_left_pose, 0 - ) - except Exception as err: - rospy.logwarn(err) - try: - # base-to-right-ee (target) + # base-to-right-ee (cartesio) t = self.tf_buffer.lookup_transform( "ci/base_link", "ci/gripper_right_grasping_frame", rospy.Time(), ) - new_target_right_pose = np.zeros((1, 8)) - new_target_right_pose[0, 0] = t.transform.translation.x - new_target_right_pose[0, 1] = t.transform.translation.y - new_target_right_pose[0, 2] = t.transform.translation.z - new_target_right_pose[0, 3] = t.transform.rotation.x - new_target_right_pose[0, 4] = t.transform.rotation.y - new_target_right_pose[0, 5] = t.transform.rotation.z - new_target_right_pose[0, 6] = t.transform.rotation.w - new_target_right_pose[0, 7] = ( - t.header.stamp.secs + 10**-9 * t.header.stamp.nsecs - ) - self.data_target_right_pose = np.append( - self.data_target_right_pose, new_target_right_pose, 0 + new_cartesio_right_pose = np.zeros((1, 8)) + new_cartesio_right_pose[0, 0] = t.transform.translation.x + new_cartesio_right_pose[0, 1] = t.transform.translation.y + new_cartesio_right_pose[0, 2] = t.transform.translation.z + new_cartesio_right_pose[0, 3] = t.transform.rotation.x + new_cartesio_right_pose[0, 4] = t.transform.rotation.y + new_cartesio_right_pose[0, 5] = t.transform.rotation.z + new_cartesio_right_pose[0, 6] = t.transform.rotation.w + new_cartesio_right_pose[0, 7] = timestamp + self.data_cartesio_right_pose = np.append( + self.data_cartesio_right_pose, new_cartesio_right_pose, 0 ) except Exception as err: - rospy.logwarn(err) + print(Fore.RED + err) + print(Style.RESET_ALL) + + def cb_left_gripper(self, msg: Float32): + if self.flg_record: + timestamp = int(datetime.now().timestamp() * 1000) + new_left_gripper_cmd = np.zeros((1, 2)) + new_left_gripper_cmd[0, 0] = msg.data + new_left_gripper_cmd[0, 1] = timestamp + self.data_left_gripper_cmd = np.append( + self.data_left_gripper_cmd, new_left_gripper_cmd, 0 + ) - rate.sleep() + def cb_right_gripper(self, msg: Float32): + if self.flg_record: + timestamp = int(datetime.now().timestamp() * 1000) + new_right_gripper_cmd = np.zeros((1, 2)) + new_right_gripper_cmd[0, 0] = msg.data + new_right_gripper_cmd[0, 1] = timestamp + self.data_right_gripper_cmd = np.append( + self.data_right_gripper_cmd, new_right_gripper_cmd, 0 + ) + + def set_state(self, state: bool): + self.flg_record = state + if state: + print(f"Start logging demo {dr.get_demo_idx()}") + else: + print(f"Stop logging demo {dr.get_demo_idx()}") + + def get_state(self): + return self.flg_record + + def get_demo_idx(self): + return self.demo_idx + + def new_demo(self): + try: + self.demo_idx += 1 + self.save_folder = os.path.join(self.folder_path, str(self.demo_idx)) + os.mkdir(self.save_folder) + print(f"New demo index = {self.demo_idx}") + except Exception as err: + print(Fore.RED + err) + print(Style.RESET_ALL) + + def cancel_log(self): + if self.flg_record: + print(Fore.YELLOW + "Cancel not allowed while log is enabled") + print(Style.RESET_ALL) + else: + print(f"Removing all npy files in {self.save_folder}/") + files = glob.glob(f"{self.save_folder}/*.npy") + for f in files: + os.remove(f) def save_data(self): - self.tf_listener.unregister() - self.sub_joint_state.unregister() - self.sub_left_gripper.unregister() - self.sub_right_gripper.unregister() + if self.flg_record: + print(Fore.YELLOW + "Saving not allowed while log is enabled") + print(Style.RESET_ALL) + else: + print(Fore.GREEN + f"Saving demo '{self.demo_idx} ...") - rospy.loginfo( - f"Finished recording, saving recorded data into '{self.save_folder}'..." - ) + np.save(f"{self.save_folder}/data_joints.npy", self.data_joints) + np.save( + f"{self.save_folder}/data_teleop_left_pose.npy", + self.data_teleop_left_pose, + ) + np.save( + f"{self.save_folder}/data_teleop_right_pose.npy", + self.data_teleop_right_pose, + ) + np.save( + f"{self.save_folder}/data_measured_left_pose.npy", + self.data_measured_left_pose, + ) + np.save( + f"{self.save_folder}/data_measured_right_pose.npy", + self.data_measured_right_pose, + ) + np.save( + f"{self.save_folder}/data_cartesio_left_pose.npy", + self.data_cartesio_left_pose, + ) + np.save( + f"{self.save_folder}/data_cartesio_right_pose.npy", + self.data_cartesio_right_pose, + ) + np.save( + f"{self.save_folder}/data_cartesio_right_pose.npy", + self.data_cartesio_right_pose, + ) + np.save( + f"{self.save_folder}/data_left_gripper_cmd.npy", + self.data_left_gripper_cmd, + ) + np.save( + f"{self.save_folder}/data_right_gripper_cmd.npy", + self.data_right_gripper_cmd, + ) + for camera in self.cameras: + for tag in self.tags: + np.save( + f"{self.save_folder}/data_pose_tag_{tag}_from_{camera}.npy", + self.data_tags[camera][int(tag)], + ) - np.save(f"{self.save_folder}/data_joints.npy", self.data_joints) - np.save( - f"{self.save_folder}/data_actual_left_pose.npy", self.data_actual_left_pose - ) - np.save( - f"{self.save_folder}/data_actual_right_pose.npy", - self.data_actual_right_pose, - ) - np.save( - f"{self.save_folder}/data_target_left_pose.npy", self.data_target_left_pose - ) - np.save( - f"{self.save_folder}/data_target_right_pose.npy", - self.data_target_right_pose, - ) - np.save( - f"{self.save_folder}/data_target_right_pose.npy", - self.data_target_right_pose, - ) - np.save(f"{self.save_folder}/data_left_gripper.npy", self.data_left_gripper) - np.save(f"{self.save_folder}/data_right_gripper.npy", self.data_right_gripper) - for tag in self.data_tags: - np.save(f"{self.save_folder}/data_pose_tag_{tag}.npy", self.data_tags[tag]) + self.init_log_variables() + + print(Fore.GREEN + "Saving complete") + print(Style.RESET_ALL) + self.new_demo() + + +def getKey(settings, timeout): + if sys.platform == "win32": + # getwch() returns a string on Windows + key = msvcrt.getwch() + else: + tty.setraw(sys.stdin.fileno()) + # sys.stdin.read() returns a string on Linux + rlist, _, _ = select([sys.stdin], [], [], timeout) + if rlist: + key = sys.stdin.read(1) + else: + key = "" + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) + return key - rospy.loginfo(f"Saving complete") +def saveTerminalSettings(): + if sys.platform == "win32": + return None + return termios.tcgetattr(sys.stdin) + + +def restoreTerminalSettings(old_settings): + if sys.platform == "win32": + return + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings) + + +def terminal_menu(dr, menu, key_timeout=0.5): + # TODO: add publish to orbbec cmd topic + settings = saveTerminalSettings() + try: + print(menu_str) + while 1: + key = getKey(settings, key_timeout) + if key == " ": + dr.set_state(not dr.get_state()) + if key == "i": + print(f"Current demo index = {dr.get_demo_idx()}") + if key == "s": + dr.save_data() + print(menu_str) + if key == "c": + dr.cancel_log() + print(menu_str) + if key == "\x03": + break + + except Exception as e: + print(e) + + finally: + restoreTerminalSettings(settings) + + +menu_str = """ + DEMO RECORDER +---------------------------------------- +'Spacebar' : start/stop demo recording +'I' : get current demo index +'S' : save recorded demo +'C' : cancel recorded demo +'Ctrl-C' : quit +---------------------------------------- +""" if __name__ == "__main__": - dr = DemoRecorder() - dr.record() - dr_thread = threading.Thread(target=dr.record) - dr_thread.start() + rospy.init_node("demo_recorder") + + # Load params + folder_path = os.environ["HOME"] + cameras = [] + tags = [] + if rospy.has_param("~self.folder_path"): + folder_path = rospy.get_param("~self.folder_path") + if rospy.has_param("~cameras"): + cameras = rospy.get_param("~cameras") + if rospy.has_param("~tags"): + tags = rospy.get_param("~tags") + + dr = DemoRecorder(folder_path, cameras, tags) + + menu_thread = threading.Thread(target=terminal_menu, args=(dr, menu_str, 0.5)) + menu_thread.start() rospy.spin() + + menu_thread.join() From b2cc565ae851ac3d0348d8546a1d48903b0be8a1 Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Sat, 19 Oct 2024 14:24:44 +0200 Subject: [PATCH 46/56] Removed demo_recorder.py Moved to orbbec camera repo --- python/demo/postprocess_demo.py | 267 ------------------ python/demo_recorder.py | 471 -------------------------------- 2 files changed, 738 deletions(-) delete mode 100644 python/demo/postprocess_demo.py delete mode 100755 python/demo_recorder.py diff --git a/python/demo/postprocess_demo.py b/python/demo/postprocess_demo.py deleted file mode 100644 index 576f5d4..0000000 --- a/python/demo/postprocess_demo.py +++ /dev/null @@ -1,267 +0,0 @@ -#!/usr/bin/env python3 - -import matplotlib.pyplot as plt -import numpy as np -import os - -joint_names = [ - "arm_left_1_joint", - "arm_left_2_joint", - "arm_left_3_joint", - "arm_left_4_joint", - "arm_left_5_joint", - "arm_left_6_joint", - "arm_left_7_joint", - "arm_right_1_joint", - "arm_right_2_joint", - "arm_right_3_joint", - "arm_right_4_joint", - "arm_right_5_joint", - "arm_right_6_joint", - "arm_right_7_joint", - "gripper_left_left_finger_joint", - "gripper_left_right_finger_joint", - "gripper_right_left_finger_joint", - "gripper_right_right_finger_joint", - "head_1_joint", - "head_2_joint", - "torso_lift_joint", - "wheel_front_left_joint", - "wheel_front_right_joint", - "wheel_rear_left_joint", - "wheel_rear_right_joint", -] - - -subf = "dummy_log" - -joints = np.load(os.path.join(subf, "data_joints.npy"))[1:, :] -print("joints.shape", joints.shape) - -actual_left_pose = np.load(os.path.join(subf, "data_actual_left_pose.npy"))[1:, :] -print("actual_left_pose.shape", actual_left_pose.shape) -actual_right_pose = np.load(os.path.join(subf, "data_actual_right_pose.npy"))[1:, :] -print("actual_right_pose.shape", actual_right_pose.shape) - -target_left_pose = np.load(os.path.join(subf, "data_target_left_pose.npy"))[1:, :] -print("target_left_pose.shape", target_left_pose.shape) -target_right_pose = np.load(os.path.join(subf, "data_target_right_pose.npy"))[1:, :] -print("target_right_pose.shape", target_right_pose.shape) - -pose_tag_0 = np.load(os.path.join(subf, "data_pose_tag_0.npy"))[1:, :] -print("pose_tag_0.shape", pose_tag_0.shape) - - -fig = plt.figure() -fig.suptitle("Left EE", fontsize=16) -plt.plot( - actual_left_pose[:, -1] - actual_left_pose[0, -1], - actual_left_pose[:, 0], - color="tab:red", - linewidth=2, - label="true x", -) -plt.plot( - target_left_pose[:, -1] - target_left_pose[0, -1], - target_left_pose[:, 0], - linestyle="--", - color="tab:red", - linewidth=2, - label="target x", -) -plt.plot( - actual_left_pose[:, -1] - actual_left_pose[0, -1], - actual_left_pose[:, 1], - color="tab:green", - linewidth=2, - label="true y", -) -plt.plot( - target_left_pose[:, -1] - target_left_pose[0, -1], - target_left_pose[:, 1], - linestyle="--", - color="tab:green", - linewidth=2, - label="target y", -) -plt.plot( - actual_left_pose[:, -1] - actual_left_pose[0, -1], - actual_left_pose[:, 2], - color="tab:blue", - linewidth=2, - label="true z", -) -plt.plot( - target_left_pose[:, -1] - target_left_pose[0, -1], - target_left_pose[:, 2], - color="tab:blue", - linestyle="--", - linewidth=2, - label="target z", -) -plt.xlabel("Time [s]") -plt.ylabel("Position [m]") -plt.legend() -plt.grid() -plt.show() - -fig = plt.figure() -fig.suptitle("Right EE", fontsize=16) -plt.plot( - actual_right_pose[:, -1] - actual_right_pose[0, -1], - actual_right_pose[:, 0], - color="tab:red", - linewidth=2, - label="true x", -) -plt.plot( - target_right_pose[:, -1] - target_right_pose[0, -1], - target_right_pose[:, 0], - linestyle="--", - color="tab:red", - linewidth=2, - label="target x", -) -plt.plot( - actual_right_pose[:, -1] - actual_right_pose[0, -1], - actual_right_pose[:, 1], - color="tab:green", - linewidth=2, - label="true y", -) -plt.plot( - target_right_pose[:, -1] - target_right_pose[0, -1], - target_right_pose[:, 1], - linestyle="--", - color="tab:green", - linewidth=2, - label="target y", -) -plt.plot( - actual_right_pose[:, -1] - actual_right_pose[0, -1], - actual_right_pose[:, 2], - color="tab:blue", - linewidth=2, - label="true z", -) -plt.plot( - target_right_pose[:, -1] - target_right_pose[0, -1], - target_right_pose[:, 2], - color="tab:blue", - linestyle="--", - linewidth=2, - label="target z", -) -plt.xlabel("Time [s]") -plt.ylabel("Position [m]") -plt.legend() -plt.grid() -plt.show() - -fig = plt.figure() -fig.suptitle("Pose Tag 0", fontsize=16) -plt.plot( - pose_tag_0[:, -1] - pose_tag_0[0, -1], - pose_tag_0[:, 0], - color="tab:red", - linewidth=2, - label="x", -) -plt.plot( - pose_tag_0[:, -1] - pose_tag_0[0, -1], - pose_tag_0[:, 1], - color="tab:green", - linewidth=2, - label="y", -) -plt.plot( - pose_tag_0[:, -1] - pose_tag_0[0, -1], - pose_tag_0[:, 2], - color="tab:blue", - linewidth=2, - label="z", -) -plt.xlabel("Time [s]") -plt.ylabel("Position [m]") -plt.legend() -plt.grid() -plt.show() - - -fig = plt.figure() -fig.suptitle("Left Gripper", fontsize=16) -plt.plot( - joints[:, -1] - joints[0, -1], - joints[:, 14], - color="tab:red", - linewidth=2, - label="left finger joint", -) -plt.plot( - joints[:, -1] - joints[0, -1], - joints[:, 15], - color="tab:green", - linewidth=2, - label="right finger joint", -) -plt.xlabel("Time [s]") -plt.ylabel("Position [m]") -plt.legend() -plt.grid() -plt.show() - -fig = plt.figure() -fig.suptitle("Right Gripper", fontsize=16) -plt.plot( - joints[:, -1] - joints[0, -1], - joints[:, 16], - color="tab:red", - linewidth=2, - label="left finger joint", -) -plt.plot( - joints[:, -1] - joints[0, -1], - joints[:, 17], - color="tab:green", - linewidth=2, - label="right finger joint", -) -plt.xlabel("Time [s]") -plt.ylabel("Position [m]") -plt.legend() -plt.grid() -plt.show() - -fig = plt.figure() -fig.suptitle("Left arm joints", fontsize=16) -plt.xlabel("Time [s]") -start_idx = 0 -for i in range(7): - plt.plot( - joints[:, -1] - joints[0, -1], - joints[:, start_idx + i], - linewidth=2, - label=joint_names[start_idx + i], - ) -plt.xlabel("Time [s]") -plt.ylabel("Angle [rad]") -plt.legend() -plt.grid() -plt.show() - -fig = plt.figure() -fig.suptitle("Right arm joints", fontsize=16) -plt.xlabel("Time [s]") -start_idx = 7 -for i in range(7): - plt.plot( - joints[:, -1] - joints[0, -1], - joints[:, start_idx + i], - linewidth=2, - label=joint_names[start_idx + i], - ) -plt.xlabel("Time [s]") -plt.ylabel("Angle [rad]") -plt.legend() -plt.grid() -plt.show() diff --git a/python/demo_recorder.py b/python/demo_recorder.py deleted file mode 100755 index 696aeeb..0000000 --- a/python/demo_recorder.py +++ /dev/null @@ -1,471 +0,0 @@ -#!/usr/bin/env python3 -from colorama import Fore, Style -from datetime import datetime -import glob -import numpy as np -import os -from select import select -import sys -import threading - -if sys.platform == "win32": - import msvcrt -else: - import termios - import tty - -import rospy -import tf2_ros -from sensor_msgs.msg import JointState -from std_msgs.msg import Float32 -from trajectory_msgs.msg import JointTrajectory -from geometry_msgs.msg import PoseStamped - - -class DemoRecorder: - def __init__(self, folder_path=os.environ["HOME"], cameras=[], tags=[]): - - # Init member variables - self.folder_path = folder_path - self.cameras = cameras - self.tags = tags - self.demo_idx = 0 - self.flg_record = False - - # Set up tf2 listener - self.tf_buffer = tf2_ros.Buffer() - self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) - - # Select new initial demo index - subfolders = [f.path for f in os.scandir(self.folder_path) if f.is_dir()] - for subf in subfolders: - try: - i = int( - subf[ - subf.startswith(os.path.join(self.folder_path, "")) - and len(os.path.join(self.folder_path, "")) : - ] - ) - if i >= self.demo_idx: - self.demo_idx = i + 1 - except: - pass - self.save_folder = os.path.join(self.folder_path, str(self.demo_idx)) - os.mkdir(self.save_folder) - - self.init_log_variables() - self.init_subscribers() - - # Wait for tf listener - rospy.sleep(1) - - print("Demo recorder initialized") - print(f"Demo index = {self.demo_idx}") - - def init_log_variables(self): - # Init joint state recorder - self.data_joints = np.zeros((1, 26)) - - # Init recorder(s) for pose tags - self.data_tags = {} - for camera in self.cameras: - self.data_tags[camera] = {} - for tag in self.tags: - self.data_tags[camera][int(tag)] = np.zeros((1, 8)) - - # Init ee pose recorder - self.data_measured_left_pose = np.zeros((1, 8)) - self.data_cartesio_left_pose = np.zeros((1, 8)) - self.data_teleop_left_pose = np.zeros((1, 8)) - self.data_teleop_right_pose = np.zeros((1, 8)) - self.data_cartesio_right_pose = np.zeros((1, 8)) - self.data_measured_right_pose = np.zeros((1, 8)) - - # Init gripper commands recorder - self.data_left_gripper_cmd = np.zeros((1, 2)) - self.data_right_gripper_cmd = np.zeros((1, 2)) - - def init_subscribers(self): - # Init joint state recorder - self.sub_joint_state = rospy.Subscriber( - "/joint_states", JointState, self.cb_joint_state - ) - - # Init recorder(s) for pose tags - for camera in self.cameras: - for tag in self.tags: - rospy.Subscriber( - f"/inria_orbbec_tags/pose_tag_{tag}", - PoseStamped, - self.cb_tag, - (camera, int(tag)), - ) - - # Init ee pose recorder - self.sub_left_teleop = rospy.Subscriber( - "/dxl_input/pos_left", PoseStamped, self.cb_left_teleop - ) - - self.sub_right_teleop = rospy.Subscriber( - "/dxl_input/pos_right", PoseStamped, self.cb_right_teleop - ) - - # Init gripper commands recorder - self.sub_left_gripper_cmd = rospy.Subscriber( - "/dxl_input/gripper_left", Float32, self.cb_left_gripper - ) - - self.sub_right_gripper_cmd = rospy.Subscriber( - "/dxl_input/gripper_right", Float32, self.cb_right_gripper - ) - - def cb_joint_state(self, msg: JointState): - """Store joints state data (format: nPoints x [j1, ..., j25, time_stamp])""" - if self.flg_record: - new_joints = np.zeros((1, 26)) - for i in range(25): - new_joints[0, i] = msg.position[i] - new_joints[0, -1] = int(datetime.now().timestamp() * 1000) # [ms] - self.data_joints = np.append(self.data_joints, new_joints, 0) - - def cb_tag(self, msg: PoseStamped, args): - """Store tags data (format: nTagsx [nPoints x [x, y, z, qx, qy, qz, qw, time_stamp]])""" - if self.flg_record: - camera_name = args[0] - tag_id = args[1] - new_tag = np.zeros((1, 8)) - new_tag[0, 0] = msg.pose.position.x - new_tag[0, 1] = msg.pose.position.y - new_tag[0, 2] = msg.pose.position.z - new_tag[0, 3] = msg.pose.orientation.x - new_tag[0, 4] = msg.pose.orientation.y - new_tag[0, 5] = msg.pose.orientation.z - new_tag[0, 6] = msg.pose.orientation.w - new_tag[0, 7] = int( - msg.header.stamp.secs * 1000 + msg.header.stamp.nsecs * 10**-6 - ) # [ms] - self.data_tags[camera_name][tag_id] = np.append( - self.data_tags[camera_name][tag_id], new_tag, 0 - ) - - def cb_left_teleop(self, msg: PoseStamped): - """Store teleop pose, cartesio and measured ee poses (format: nPoints x [x, y, z, qx, qy, qz, qw, time_stamp])""" - if self.flg_record: - try: - timestamp = int(datetime.now().timestamp() * 1000) - - # base-to-left-ee (teleop) - new_teleop_left_pose = np.zeros((1, 8)) - new_teleop_left_pose[0, 0] = msg.pose.position.x - new_teleop_left_pose[0, 1] = msg.pose.position.y - new_teleop_left_pose[0, 2] = msg.pose.position.z - new_teleop_left_pose[0, 3] = msg.pose.orientation.x - new_teleop_left_pose[0, 4] = msg.pose.orientation.y - new_teleop_left_pose[0, 5] = msg.pose.orientation.z - new_teleop_left_pose[0, 6] = msg.pose.orientation.w - new_teleop_left_pose[0, 7] = timestamp - self.data_teleop_left_pose = np.append( - self.data_teleop_left_pose, new_teleop_left_pose, 0 - ) - - # base-to-left-ee (measured) - t = self.tf_buffer.lookup_transform( - "base_link", - "gripper_left_grasping_frame", - rospy.Time(), - ) - new_measured_left_pose = np.zeros((1, 8)) - new_measured_left_pose[0, 0] = t.transform.translation.x - new_measured_left_pose[0, 1] = t.transform.translation.y - new_measured_left_pose[0, 2] = t.transform.translation.z - new_measured_left_pose[0, 3] = t.transform.rotation.x - new_measured_left_pose[0, 4] = t.transform.rotation.y - new_measured_left_pose[0, 5] = t.transform.rotation.z - new_measured_left_pose[0, 6] = t.transform.rotation.w - new_measured_left_pose[0, 7] = timestamp - self.data_measured_left_pose = np.append( - self.data_measured_left_pose, new_measured_left_pose, 0 - ) - - # base-to-left-ee (cartesio) - t = self.tf_buffer.lookup_transform( - "ci/base_link", - "ci/gripper_left_grasping_frame", - rospy.Time(), - ) - new_cartesio_left_pose = np.zeros((1, 8)) - new_cartesio_left_pose[0, 0] = t.transform.translation.x - new_cartesio_left_pose[0, 1] = t.transform.translation.y - new_cartesio_left_pose[0, 2] = t.transform.translation.z - new_cartesio_left_pose[0, 3] = t.transform.rotation.x - new_cartesio_left_pose[0, 4] = t.transform.rotation.y - new_cartesio_left_pose[0, 5] = t.transform.rotation.z - new_cartesio_left_pose[0, 6] = t.transform.rotation.w - new_cartesio_left_pose[0, 7] = timestamp - self.data_cartesio_left_pose = np.append( - self.data_cartesio_left_pose, new_cartesio_left_pose, 0 - ) - except Exception as err: - print(Fore.RED + err) - print(Style.RESET_ALL) - - def cb_right_teleop(self, msg: PoseStamped): - """Store teleop pose, cartesio and measured ee poses (format: nPoints x [x, y, z, qx, qy, qz, qw, time_stamp])""" - if self.flg_record: - try: - timestamp = int(datetime.now().timestamp() * 1000) - - # base-to-right-ee (teleop) - new_teleop_right_pose = np.zeros((1, 8)) - new_teleop_right_pose[0, 0] = msg.pose.position.x - new_teleop_right_pose[0, 1] = msg.pose.position.y - new_teleop_right_pose[0, 2] = msg.pose.position.z - new_teleop_right_pose[0, 3] = msg.pose.orientation.x - new_teleop_right_pose[0, 4] = msg.pose.orientation.y - new_teleop_right_pose[0, 5] = msg.pose.orientation.z - new_teleop_right_pose[0, 6] = msg.pose.orientation.w - new_teleop_right_pose[0, 7] = timestamp - self.data_teleop_right_pose = np.append( - self.data_teleop_right_pose, new_teleop_right_pose, 0 - ) - - # base-to-right-ee (measured) - t = self.tf_buffer.lookup_transform( - "base_link", - "gripper_right_grasping_frame", - rospy.Time(), - ) - new_measured_right_pose = np.zeros((1, 8)) - new_measured_right_pose[0, 0] = t.transform.translation.x - new_measured_right_pose[0, 1] = t.transform.translation.y - new_measured_right_pose[0, 2] = t.transform.translation.z - new_measured_right_pose[0, 3] = t.transform.rotation.x - new_measured_right_pose[0, 4] = t.transform.rotation.y - new_measured_right_pose[0, 5] = t.transform.rotation.z - new_measured_right_pose[0, 6] = t.transform.rotation.w - new_measured_right_pose[0, 7] = timestamp - self.data_measured_right_pose = np.append( - self.data_measured_right_pose, new_measured_right_pose, 0 - ) - - # base-to-right-ee (cartesio) - t = self.tf_buffer.lookup_transform( - "ci/base_link", - "ci/gripper_right_grasping_frame", - rospy.Time(), - ) - new_cartesio_right_pose = np.zeros((1, 8)) - new_cartesio_right_pose[0, 0] = t.transform.translation.x - new_cartesio_right_pose[0, 1] = t.transform.translation.y - new_cartesio_right_pose[0, 2] = t.transform.translation.z - new_cartesio_right_pose[0, 3] = t.transform.rotation.x - new_cartesio_right_pose[0, 4] = t.transform.rotation.y - new_cartesio_right_pose[0, 5] = t.transform.rotation.z - new_cartesio_right_pose[0, 6] = t.transform.rotation.w - new_cartesio_right_pose[0, 7] = timestamp - self.data_cartesio_right_pose = np.append( - self.data_cartesio_right_pose, new_cartesio_right_pose, 0 - ) - except Exception as err: - print(Fore.RED + err) - print(Style.RESET_ALL) - - def cb_left_gripper(self, msg: Float32): - if self.flg_record: - timestamp = int(datetime.now().timestamp() * 1000) - new_left_gripper_cmd = np.zeros((1, 2)) - new_left_gripper_cmd[0, 0] = msg.data - new_left_gripper_cmd[0, 1] = timestamp - self.data_left_gripper_cmd = np.append( - self.data_left_gripper_cmd, new_left_gripper_cmd, 0 - ) - - def cb_right_gripper(self, msg: Float32): - if self.flg_record: - timestamp = int(datetime.now().timestamp() * 1000) - new_right_gripper_cmd = np.zeros((1, 2)) - new_right_gripper_cmd[0, 0] = msg.data - new_right_gripper_cmd[0, 1] = timestamp - self.data_right_gripper_cmd = np.append( - self.data_right_gripper_cmd, new_right_gripper_cmd, 0 - ) - - def set_state(self, state: bool): - self.flg_record = state - if state: - print(f"Start logging demo {dr.get_demo_idx()}") - else: - print(f"Stop logging demo {dr.get_demo_idx()}") - - def get_state(self): - return self.flg_record - - def get_demo_idx(self): - return self.demo_idx - - def new_demo(self): - try: - self.demo_idx += 1 - self.save_folder = os.path.join(self.folder_path, str(self.demo_idx)) - os.mkdir(self.save_folder) - print(f"New demo index = {self.demo_idx}") - except Exception as err: - print(Fore.RED + err) - print(Style.RESET_ALL) - - def cancel_log(self): - if self.flg_record: - print(Fore.YELLOW + "Cancel not allowed while log is enabled") - print(Style.RESET_ALL) - else: - print(f"Removing all npy files in {self.save_folder}/") - files = glob.glob(f"{self.save_folder}/*.npy") - for f in files: - os.remove(f) - - def save_data(self): - if self.flg_record: - print(Fore.YELLOW + "Saving not allowed while log is enabled") - print(Style.RESET_ALL) - else: - print(Fore.GREEN + f"Saving demo '{self.demo_idx} ...") - - np.save(f"{self.save_folder}/data_joints.npy", self.data_joints) - np.save( - f"{self.save_folder}/data_teleop_left_pose.npy", - self.data_teleop_left_pose, - ) - np.save( - f"{self.save_folder}/data_teleop_right_pose.npy", - self.data_teleop_right_pose, - ) - np.save( - f"{self.save_folder}/data_measured_left_pose.npy", - self.data_measured_left_pose, - ) - np.save( - f"{self.save_folder}/data_measured_right_pose.npy", - self.data_measured_right_pose, - ) - np.save( - f"{self.save_folder}/data_cartesio_left_pose.npy", - self.data_cartesio_left_pose, - ) - np.save( - f"{self.save_folder}/data_cartesio_right_pose.npy", - self.data_cartesio_right_pose, - ) - np.save( - f"{self.save_folder}/data_cartesio_right_pose.npy", - self.data_cartesio_right_pose, - ) - np.save( - f"{self.save_folder}/data_left_gripper_cmd.npy", - self.data_left_gripper_cmd, - ) - np.save( - f"{self.save_folder}/data_right_gripper_cmd.npy", - self.data_right_gripper_cmd, - ) - for camera in self.cameras: - for tag in self.tags: - np.save( - f"{self.save_folder}/data_pose_tag_{tag}_from_{camera}.npy", - self.data_tags[camera][int(tag)], - ) - - self.init_log_variables() - - print(Fore.GREEN + "Saving complete") - print(Style.RESET_ALL) - self.new_demo() - - -def getKey(settings, timeout): - if sys.platform == "win32": - # getwch() returns a string on Windows - key = msvcrt.getwch() - else: - tty.setraw(sys.stdin.fileno()) - # sys.stdin.read() returns a string on Linux - rlist, _, _ = select([sys.stdin], [], [], timeout) - if rlist: - key = sys.stdin.read(1) - else: - key = "" - termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) - return key - - -def saveTerminalSettings(): - if sys.platform == "win32": - return None - return termios.tcgetattr(sys.stdin) - - -def restoreTerminalSettings(old_settings): - if sys.platform == "win32": - return - termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings) - - -def terminal_menu(dr, menu, key_timeout=0.5): - # TODO: add publish to orbbec cmd topic - settings = saveTerminalSettings() - try: - print(menu_str) - while 1: - key = getKey(settings, key_timeout) - if key == " ": - dr.set_state(not dr.get_state()) - if key == "i": - print(f"Current demo index = {dr.get_demo_idx()}") - if key == "s": - dr.save_data() - print(menu_str) - if key == "c": - dr.cancel_log() - print(menu_str) - if key == "\x03": - break - - except Exception as e: - print(e) - - finally: - restoreTerminalSettings(settings) - - -menu_str = """ - DEMO RECORDER ----------------------------------------- -'Spacebar' : start/stop demo recording -'I' : get current demo index -'S' : save recorded demo -'C' : cancel recorded demo -'Ctrl-C' : quit ----------------------------------------- -""" - -if __name__ == "__main__": - rospy.init_node("demo_recorder") - - # Load params - folder_path = os.environ["HOME"] - cameras = [] - tags = [] - if rospy.has_param("~self.folder_path"): - folder_path = rospy.get_param("~self.folder_path") - if rospy.has_param("~cameras"): - cameras = rospy.get_param("~cameras") - if rospy.has_param("~tags"): - tags = rospy.get_param("~tags") - - dr = DemoRecorder(folder_path, cameras, tags) - - menu_thread = threading.Thread(target=terminal_menu, args=(dr, menu_str, 0.5)) - menu_thread.start() - - rospy.spin() - - menu_thread.join() From 89efe9586ba1727aa50c86bcdcc7c51ca92fb416 Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Thu, 24 Oct 2024 13:50:43 +0200 Subject: [PATCH 47/56] Removed postural gui from cartesio.launch --- launch/cartesio.launch | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/launch/cartesio.launch b/launch/cartesio.launch index fbcd6f9..bfb173c 100644 --- a/launch/cartesio.launch +++ b/launch/cartesio.launch @@ -1,6 +1,4 @@ - - @@ -57,11 +55,5 @@ - - - - - - - + From 6da16c742f5f79ea5bb73f6f6b5470482b8cf235 Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Tue, 29 Oct 2024 10:10:23 +0100 Subject: [PATCH 48/56] Changed camera_head_frame transformation --- capsules/urdf/tiago_dual_capsules.rviz | 2 +- robots/tiago_dual.urdf.xacro | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/capsules/urdf/tiago_dual_capsules.rviz b/capsules/urdf/tiago_dual_capsules.rviz index 9557200..f6455f3 100644 --- a/capsules/urdf/tiago_dual_capsules.rviz +++ b/capsules/urdf/tiago_dual_capsules.rviz @@ -2359,7 +2359,7 @@ - + diff --git a/robots/tiago_dual.urdf.xacro b/robots/tiago_dual.urdf.xacro index d3555b8..7dfd9b7 100644 --- a/robots/tiago_dual.urdf.xacro +++ b/robots/tiago_dual.urdf.xacro @@ -26,7 +26,7 @@ - + From 91f0bfa115ea001d1376d5352526381bc0899cca Mon Sep 17 00:00:00 2001 From: Fabio Amadio Date: Tue, 29 Oct 2024 18:34:14 +0100 Subject: [PATCH 49/56] Reduced postural lambda --- stack/tiago_dual.stack | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stack/tiago_dual.stack b/stack/tiago_dual.stack index 3467373..bbd1783 100644 --- a/stack/tiago_dual.stack +++ b/stack/tiago_dual.stack @@ -23,7 +23,7 @@ Ree: Postural: type: "Postural" - lambda: 0.1 + lambda: 0.05 use_inertia: false weight: reference@v0: 0.0 From 4d15d9fd6ae314ec73515e02313e39d8b6938e2e Mon Sep 17 00:00:00 2001 From: Clemente Donoso Date: Tue, 3 Dec 2024 08:59:29 +0100 Subject: [PATCH 50/56] Add joystick controller --- launch/joystick_controller.launch | 16 +++++++ python/joystick_controller.py | 74 +++++++++++++++++++++++++++++++ 2 files changed, 90 insertions(+) create mode 100644 launch/joystick_controller.launch create mode 100644 python/joystick_controller.py diff --git a/launch/joystick_controller.launch b/launch/joystick_controller.launch new file mode 100644 index 0000000..f2a3f0d --- /dev/null +++ b/launch/joystick_controller.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/python/joystick_controller.py b/python/joystick_controller.py new file mode 100644 index 0000000..553004d --- /dev/null +++ b/python/joystick_controller.py @@ -0,0 +1,74 @@ +#!/usr/bin/env python3 + +from cartesian_interface.pyci_all import * +import rospy +from sensor_msgs.msg import Joy +import sys +import numpy as np +from std_msgs.msg import Float32 + +class JoystickController: + def __init__(self): + rospy.init_node("joystick_controller") + + self.joy_topic = rospy.get_param("~joy_topic", "/tiago_dual_cartesio/joy") + self.tf_prefix = rospy.get_param("~tf_prefix", "ci") + self.lin_scale = rospy.get_param("~lin_scale", 0.2) + self.ang_scale = rospy.get_param("~ang_scale", 1.0) + + self.joy_sub = rospy.Subscriber(self.joy_topic, Joy, self.joy_callback) + self.ros_control_publisher = rospy.Publisher("/ros_control_bridge/time_from_start", Float32, queue_size=1) + + self.ros_control_publisher.publish(0.6) + + rospy.loginfo("Joystick controller initialized, Settings:") + rospy.loginfo("Joy topic: %s", self.joy_topic) + rospy.loginfo("TF prefix: %s", self.tf_prefix) + rospy.loginfo("Linear scale: %s", self.lin_scale) + rospy.loginfo("Angular scale: %s", self.ang_scale) + + try: + self.cli = pyci.CartesianInterfaceRos() + except Exception as e: + rospy.logerr("No Cartesio server found. Shutting down.") + sys.exit() + + self.task = self.cli.getTask("base_link") + + self.task.setControlMode(pyci.ControlType.Velocity) + + def joy_callback(self, msg): + vel_x = msg.axes[1] * self.lin_scale + vel_y = msg.axes[0] * self.lin_scale + vel_ang = msg.axes[3] * self.ang_scale + + deadman_button = msg.buttons[7] + + if deadman_button == 1: + ref = np.array([vel_x, vel_y, 0.0, 0.0, 0.0, vel_ang]) + + H = self.cli.getPoseFromTf( + self.tf_prefix + "/" + self.task.getName(), + self.tf_prefix + "/" + self.task.getBaseLink(), + ) + + adj = np.block( + [ + [H.linear, np.zeros((3, 3))], + [np.zeros((3, 3)), H.linear], + ] + ) + + ref = adj @ ref + + self.task.setVelocityReference(ref) + + + + +if __name__ == "__main__": + jc = JoystickController() + rospy.spin() + + + \ No newline at end of file From e914066a1c7f795f094158bcaa931ff94fadcf7a Mon Sep 17 00:00:00 2001 From: Clemente Donoso Date: Mon, 13 Jan 2025 11:45:03 +0100 Subject: [PATCH 51/56] Remove launch for demo recorder since the demo_recorder.py was removed --- launch/demo_recorder.launch | 7 ------- 1 file changed, 7 deletions(-) delete mode 100644 launch/demo_recorder.launch diff --git a/launch/demo_recorder.launch b/launch/demo_recorder.launch deleted file mode 100644 index 942ff04..0000000 --- a/launch/demo_recorder.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - "/home/forest_ws/src/tiago_dual_cartesio_config/python/demo" - ["orbbec_head"] - [0] - - From 95660b18fa2d5ae3f40c669d930d8fe00b0b6922 Mon Sep 17 00:00:00 2001 From: Clemente Donoso Date: Mon, 13 Jan 2025 12:07:11 +0100 Subject: [PATCH 52/56] Update readme instructions --- README.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 907f62f..ee57a33 100644 --- a/README.md +++ b/README.md @@ -29,13 +29,14 @@ Then run the controller using: ```reset && mon launch tiago_dual_cartesio_config/launch/cartesio.launch``` -Connect to Teleoperation device +How to move the robot ------------------------------- First, perform the steps in **How to run on the robot**. Then run the launch file: ```reset && mon launch tiago_dual_cartesio_config teleop.launch``` -Notice that the ```teleop.launch``` contains two Python nodes that can be replicated in order to control both left and right arms and grippers. +This will start the teleoperation node that allows to control the arms and the gripper for the robot. Then to move the base of the robot run the launch file: +```reset && mon launch tiago_dual_cartesio_config joystick_controller.launch``` From 2f66f6a244f280896ef1324d2d6759d9f1d95473 Mon Sep 17 00:00:00 2001 From: Clemente Donoso Date: Tue, 14 Jan 2025 16:17:58 +0100 Subject: [PATCH 53/56] Add argument to use postural_gui related to the reviews given by Enrico --- launch/cartesio.launch | 3 +++ 1 file changed, 3 insertions(+) diff --git a/launch/cartesio.launch b/launch/cartesio.launch index bfb173c..3cdb318 100644 --- a/launch/cartesio.launch +++ b/launch/cartesio.launch @@ -55,5 +55,8 @@ + + + From 25435839a3dd1e7df5d128dcc9e669848f4dbdeb Mon Sep 17 00:00:00 2001 From: Clemente Donoso Date: Tue, 14 Jan 2025 16:20:29 +0100 Subject: [PATCH 54/56] Update the constraints to have Omni4X and the stack to have Gaze --- stack/tiago_dual.stack | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/stack/tiago_dual.stack b/stack/tiago_dual.stack index bbd1783..7221b90 100644 --- a/stack/tiago_dual.stack +++ b/stack/tiago_dual.stack @@ -3,11 +3,11 @@ solver_options: back_end: "qpoases" stack: - - ["Lee", "Ree", "Base"] + - ["Lee", "Ree", "Base", "Gaze"] - ["Postural"] -constraints: ["JointLimits", "VelocityLimits", "Base2D", "Collision"] +constraints: ["JointLimits", "VelocityLimits", "Base2D", "Omni4X", "Collision"] Lee: type: "Cartesian" From 4a640fdab6d4537a875cbb3be6d1bca62ebf0421 Mon Sep 17 00:00:00 2001 From: Clemente Donoso Date: Thu, 16 Jan 2025 15:23:22 +0100 Subject: [PATCH 55/56] Add definition of Gaze and Omni4X to the stack --- stack/tiago_dual.stack | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/stack/tiago_dual.stack b/stack/tiago_dual.stack index 7221b90..1a36d8e 100644 --- a/stack/tiago_dual.stack +++ b/stack/tiago_dual.stack @@ -3,11 +3,25 @@ solver_options: back_end: "qpoases" stack: - - ["Lee", "Ree", "Base", "Gaze"] + - ["Lee", "Ree", "Base"] - ["Postural"] - -constraints: ["JointLimits", "VelocityLimits", "Base2D", "Omni4X", "Collision"] +Gaze: + type: "Gaze" + distal_link: "head_front_camera_link" + base_link: "base_link" + lambda: 1. + lambda: 0.1 + +constraints: ["JointLimits", "VelocityLimits", "Base2D", "Collision"] + +Omni4X: + type: "OmniWheels4X" + l1: 0.223 + l2: 0.244 + wheel_radius: 0.08 + base_link: "base_link" + joint_wheels_name: ["wheel_front_left_joint", "wheel_front_right_joint", "wheel_rear_left_joint", "wheel_rear_right_joint"] Lee: type: "Cartesian" From b351c51472bbba1d57cc5fc5cb4fceeed455f545 Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Fri, 17 Jan 2025 09:11:26 +0100 Subject: [PATCH 56/56] small change in stack file --- stack/tiago_dual.stack | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/stack/tiago_dual.stack b/stack/tiago_dual.stack index 1a36d8e..e77048c 100644 --- a/stack/tiago_dual.stack +++ b/stack/tiago_dual.stack @@ -6,6 +6,8 @@ stack: - ["Lee", "Ree", "Base"] - ["Postural"] +constraints: ["JointLimits", "VelocityLimits", "Base2D", "Collision"] + Gaze: type: "Gaze" distal_link: "head_front_camera_link" @@ -13,8 +15,6 @@ Gaze: lambda: 1. lambda: 0.1 -constraints: ["JointLimits", "VelocityLimits", "Base2D", "Collision"] - Omni4X: type: "OmniWheels4X" l1: 0.223