diff --git a/harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics_arm.py b/harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics_arm.py index 4125b3c..96ee620 100644 --- a/harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics_arm.py +++ b/harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics_arm.py @@ -31,12 +31,22 @@ class KinematicsSolver(Node): self.robot_kine_rm.add_tool_frames(self.config['tools_in_ee']) self.robot_kine_rm.cfg_j_limit(min_j=self.config['min_joint'], max_j=self.config['max_joint'], rad_flag=False) - self.action_arm_pub = self.create_publisher(JointState, self.config["observation_topic"], 10) + # pub, arm joint command + self.action_arm_pub = self.create_publisher(JointState, self.config["action_arm_topic"], 10) - # Create subcriber for target joint + # sub, get the desired pose command in task frame self.action_pose_sub = self.create_subscription(PoseStamped, self.config["action_pose_topic"], self.pose_callback, 10) + # sub, get the desired tool command self.action_tool_sub = self.create_subscription(Bool, self.config["action_tool_topic"], self.tool_callback, 10) + # sub, get the teleoperation enable command self.action_enable_sub = self.create_subscription(Bool, self.config["action_arm_enable_topic"], self.arm_enable_callback, 10) + # sub, get the joint sts from arm + self.obser_arm_sub = self.create_subscription(JointState, self.config["observation_topic"], self.arm_observation_callback, 10) + + self.timer = self.create_timer( + self.config["interval"], + self.kine_timer + ) self.get_logger().info(f'the config is {self.config}') @@ -65,13 +75,15 @@ class KinematicsSolver(Node): "bound_cartesian": self.declare_parameter('bound_cartesian', [-0.3, -0.5, -0.1, 0.3, 0.5, 0.6]).value, - "arm_installation": self.declare_parameter('arm_installation', [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).value, + "arm_installation": self.declare_parameter('arm_installation', [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]).value, "tool_name": tool_name, "tools_in_ee": tools_in_ee, "interval": self.declare_parameter('interval', 0.02).value, + "urdf_path": self.declare_parameter('urdf_path', "/home/zl/Downloads/urdf_rm75/RM75-B.urdf").value, "mesh_dir": self.declare_parameter('mesh_dir', "/home/zl/Downloads/urdf_rm75").value, + "action_arm_topic": self.declare_parameter('action_topic', "/action/arm").value, "observation_topic": self.declare_parameter('observation_topic', "/observation/arm").value, "action_pose_topic": self.declare_parameter('action_pose_topic', "/action/pose").value, @@ -87,7 +99,7 @@ class KinematicsSolver(Node): "tool_cmd": 0, "pose_sts": [0.0] * 6, "pose_target_timestamp": 0.0, - "ik_ret": False, + "ik_ret": 0, } return config, sts @@ -95,10 +107,13 @@ class KinematicsSolver(Node): def update_msg(self): msg = JointState() msg.header.stamp = self.get_clock().now().to_msg() - msg.name = ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7'] - msg.position = [float(self.sts["arm_enable"])] + self.sts["joint_solved"] + [float(self.sts["tool_cmd"])] + msg.position = [float(self.sts["arm_enable"])] + [float(self.sts["ik_ret"])] + self.sts["joint_solved"] + [float(self.sts["tool_cmd"])] return msg + def kine_timer(self): + msg = self.update_msg() + self.action_arm_pub.publish(msg) + def arm_enable_callback(self, msg): if msg.data: self.sts["arm_enable"] = 1 @@ -111,6 +126,12 @@ class KinematicsSolver(Node): else: self.sts["tool_cmd"] = 0 + def arm_observation_callback(self, msg): + if isinstance(msg, JointState): + self.sts["joint_pos_sts"] = list(msg.position[2:9]) + else: + self.get_logger().error("Received message of unsupported type.") + def pose_callback(self, msg): if isinstance(msg, PoseStamped): timestamp = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9 @@ -136,22 +157,27 @@ class KinematicsSolver(Node): ret_rm_ik, q_out = self.robot_kine_rm.inverse_kinematics(target_position, target_rpy, initial_guess, tool) if ret_rm_ik == 0: self.sts["joint_solved"] = q_out - self.sts["ik_ret"] = True + self.sts["ik_ret"] = 1 else: ret_qp_ik, q_out = self.robot_kine_qp.inverse_kinematics(target_position= target_position, target_rpy=target_rpy, initial_guess=initial_guess, tool=tool, max_iter=300, work=work) if ret_qp_ik == 0: self.sts["joint_solved"] = q_out - self.sts["ik_ret"] = True + self.sts["ik_ret"] = 1 else: - self.sts["ik_ret"] = False + self.sts["ik_ret"] = 0 def pose_transform(self, pos, quat, pos_frame, quat_frame): ''' pos: position in the task frame quat: quaternion in the task frame - pos_frame: position in the arm base frame - quat_frame: quaternion in the arm base frame + pos_frame: task frame position in the arm base frame + quat_frame: task framequaternion in the arm base frame ''' + pos_frame = np.asarray(pos_frame, dtype=np.float32) + quat_frame = np.asarray(quat_frame, dtype=np.float32) + pos = np.asarray(pos, dtype=np.float32) + quat = np.asarray(quat, dtype=np.float32) + pos_arm = rotate_vector_by_quaternion( quat=quat_frame, v=pos) + pos_frame quat_arm = quaternion_multiply(quat_frame, quat) return pos_arm, quat_arm diff --git a/harvest_arm_rm/config/config.yaml b/harvest_arm_rm/config/config.yaml index b514d5c..db05140 100644 --- a/harvest_arm_rm/config/config.yaml +++ b/harvest_arm_rm/config/config.yaml @@ -19,21 +19,24 @@ harvest_arm_ctrl: ros__parameters: - pos_task_in_base = [-0.1, 0.2, -0.025] - quat_task_in_base = [-0.5, -0.5, 0.5, 0.5] + - work_frame: ["base", "task"] - tool_names: ["scissor", "omnipic", "minisci", "vacuum", "no_tool"] + # set the position and quaternion of the task frame in the base frame. + arm_installation : [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] + + + tool_names: ["scissor", "omnipic", "minisci", "camera", "gripper_bh", "no_tool"] qp_iteration: 200 arm_installation: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -# 0~2 position, x, y, z, m -# 3~6 orientation, quaternion, x, y, z, w -# 7 mass, kg -# 8~10 center of mass, x, y, z, m + # The tool parameters are defined in the following order: + # 0~2 position, x, y, z, m + # 3~6 orientation, quaternion, x, y, z, w + # 7 mass, kg + # 8~10 center of mass, x, y, z, m tools_in_ee.scissor: [ 0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0, 0.66, 0.0, 0.0, 0.06 @@ -49,6 +52,16 @@ harvest_arm_ctrl: 0.46, 0.0, 0.0, 0.06 ] + tools_in_ee.camera: [ + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, + 0.0, 0.0, 0.0, 0.0 + ] + + tools_in_ee.gripper_bh: [ + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, + 0.0, 0.0, 0.0, 0.0 + ] + tools_in_ee.no_tool: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0 diff --git a/harvest_arm_rm/harvest_arm_rm/run_realman_arm.py b/harvest_arm_rm/harvest_arm_rm/run_realman_arm.py index 7e14c98..feac5fb 100644 --- a/harvest_arm_rm/harvest_arm_rm/run_realman_arm.py +++ b/harvest_arm_rm/harvest_arm_rm/run_realman_arm.py @@ -66,11 +66,11 @@ class RM_Arm(Node): dict: Configuration dictionary containing robot IP, host IP, prefix, left/right status, port, savefile flag, and scissor gripper status. """ tool_name = self.declare_parameter('tool_name', "no_tool").value - tool_name_param = 'tools_in_ee.' + tool_name - try: - tool_offset_list = self.declare_parameter(tool_name_param, [0.0] * 11).value - except: - self.get_logger().error(f"Tool name '{tool_name}' not found in parameters. Please check the configuration.") + # tool_name_param = 'tools_in_ee.' + tool_name + # try: + # tool_offset_list = self.declare_parameter(tool_name_param, [0.0] * 11).value + # except: + # self.get_logger().error(f"Tool name '{tool_name}' not found in parameters. Please check the configuration.") acc_lmt = self.declare_parameter('max_joint_acc', 600.0).value @@ -91,12 +91,12 @@ class RM_Arm(Node): "max_cartesian_angular_speed": self.declare_parameter('max_cartesian_angular_speed', 1.5).value, "max_cartesian_acc": self.declare_parameter('max_cartesian_acc', 1.5).value, "max_cartesian_angular_acc": self.declare_parameter('max_cartesian_angular_acc', 2.0).value, - "bound_cartesian": self.declare_parameter('bound_cartesian', [-0.3, -0.5, -0.1, 0.3, 0.5, 0.6]).value, + # "bound_cartesian": self.declare_parameter('bound_cartesian', [-0.3, -0.5, -0.1, 0.3, 0.5, 0.6]).value, - "arm_installation": self.declare_parameter('arm_installation', [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).value, + # "arm_installation": self.declare_parameter('arm_installation', [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).value, "tool_name": tool_name, - "tool_offset": tool_offset_list, - "tool_names": self.declare_parameter('tool_names', ["scissor", "omnipic", "minisci", "no_tool"]).value, + # "tool_offset": tool_offset_list, + # "tool_names": self.declare_parameter('tool_names', ["scissor", "omnipic", "minisci", "no_tool"]).value, "initial_joint": self.declare_parameter('initial_joint', [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).value, "kp_j": self.declare_parameter('kp_j', 1.0).value, @@ -113,12 +113,20 @@ class RM_Arm(Node): "arm_tool_io_mode": self.declare_parameter('arm_tool_io_mode', [1, 1]).value } + # joint_cmd: the joint command to the arm, unit: degree; + # joint_target: the joint target from the /action/arm, unit: degree; + # joint_pos_sts: the joint position from the arm udp (real robot) or from the /observation/arm_rviz (if no real robot), unit: degree; + # joint_speed_sts: the joint speed from the arm udp (real robot) or from the /observation/arm_rviz (if no real robot), unit: degree/s; + # arm_enable: 1 or 0, the arm enable status from the /action/arm; + # tool_cmd: the tool command from the /action/arm, 1 or 0; + # pose_sts: the pose from the arm udp (real robot), unit: m and rad; sts = { "joint_cmd": [0.0] * 7, "joint_target": [0.0] * 7, "joint_pos_sts": [0.0] * 7, "joint_speed_sts": [0.0] * 7, "arm_enable": int(0), + "ik_ret": int(0), "tool_cmd": [int(0), int(0)], "pose_sts": [0.0] * 6, "timer_cnt": 0, @@ -157,13 +165,14 @@ class RM_Arm(Node): action_list = list(msg.position) self.sts["arm_enable"] = int(action_list[0]) - self.sts["joint_target"] = action_list[1:8] - self.sts["tool_cmd"][0] = int(action_list[8]) + self.sts["ik_ret"] = int(action_list[1]) + self.sts["joint_target"] = action_list[2:9] + self.sts["tool_cmd"][0] = int(action_list[9]) def arm_observation_relay_pub(self): """ - Publish the current state of the robotic arm. + Publish the current state of the robotic arm to rviz. """ msg0= JointState() msg0.header.stamp = self.get_clock().now().to_msg() @@ -173,7 +182,9 @@ class RM_Arm(Node): self.action_rviz_pub.publish(msg0) - + ''' + publish the joint state to the harvest_arm_ctrl pkg. + ''' msg = JointState() msg.header.stamp = self.get_clock().now().to_msg() arm_real = self.arm_arg['arm_handle'].id @@ -283,9 +294,9 @@ class RM_Arm(Node): send joint command to the arm """ ############# movej --> arm - if self.sts["arm_enable"] == 1: + if self.sts["arm_enable"] == 1 and self.sts["ik_ret"] == 1: dis_max_vt = self.config["max_joint_speed"] * self.config["interval"] - dis_vt = np.array(self.sts["joint_speed_sts"]) * self.config["interval"] * 0.75 + dis_vt = np.array(self.sts["joint_speed_sts"]) * self.config["interval"] * 0.8 d_j = np.clip(a=(np.array(self.sts["joint_target"]) - np.array(self.sts["joint_pos_sts"])) * self.config["kp_j"], a_min= dis_vt - self.config["lmt_05att"] , a_max=dis_vt + self.config["lmt_05att"]) @@ -314,7 +325,7 @@ class RM_Arm(Node): arm (RoboticArm): Instance of the RoboticArm class. config (dict): Configuration dictionary containing tool offsets. """ - self.frame_cfg() + # self.frame_cfg() self.connector_cfg() diff --git a/harvest_arm_rm/launch/rm_arm_rviz.launch.py b/harvest_arm_rm/launch/rm_arm_rviz.launch.py index d01b727..55f72d0 100644 --- a/harvest_arm_rm/launch/rm_arm_rviz.launch.py +++ b/harvest_arm_rm/launch/rm_arm_rviz.launch.py @@ -89,7 +89,7 @@ header: nanosec: $(date +%N) frame_id: '' name: [] -position: [1.0, 82.6, -20.28, 32.15, -50.28, -17.28, -64.055, 20.62, 1.0] +position: [1.0, 1.0, 82.6, -20.28, 32.15, -50.28, -17.28, -64.055, 20.62, 1.0] velocity: [] effort: [] " --once