From 0e97be92638b9ee1539278ca7aa69842cc7032a1 Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Wed, 21 Oct 2015 19:51:09 +0200 Subject: [PATCH] arm_ros_conn: allow to preempt --- scripts/arm_ros_conn.py | 41 +++++++++++++++++++++++++++++++++-------- urdf/arm.urdf | 6 +++--- 2 files changed, 36 insertions(+), 11 deletions(-) diff --git a/scripts/arm_ros_conn.py b/scripts/arm_ros_conn.py index 42a3e56..1d27cd5 100755 --- a/scripts/arm_ros_conn.py +++ b/scripts/arm_ros_conn.py @@ -4,9 +4,11 @@ import rospy import arm import actionlib +import numpy as np from sensor_msgs.msg import JointState from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryActionResult, FollowJointTrajectoryActionFeedback, FollowJointTrajectoryResult from actionlib_msgs.msg import GoalStatus +from time import sleep from math import * lJointNames = ["base_to_link1", "link_1_2_joint", "link_2_3_joint", "gripper_joint_1", "gripper_joint_2", "left_gripper_joint", "right_gripper_joint"] @@ -20,6 +22,7 @@ class ARMRosConn(): rospy.init_node('arm') self.speed = 220 + self.lAngles = [0] * 6 arm.switch(0) arm.switch(2) arm.set_hall_mode(3, 0) @@ -42,21 +45,43 @@ class ARMRosConn(): joint_state = JointState() joint_state.header.stamp = rospy.Time.now() joint_state.name = lJointNames - joint_state.position = [-arm.get_angle(0), arm.get_angle(1), -arm.get_angle(2), -arm.get_angle(3), arm.get_angle(4), 0.175-arm.get_angle(5)/2, 0.175-arm.get_angle(5)/2] + self.lAngles = [-arm.get_angle(0), arm.get_angle(1), -arm.get_angle(2), -arm.get_angle(3), arm.get_angle(4), arm.get_angle(5)] + joint_state.position = self.lAngles[:-1] + [0.175-self.lAngles[-1]/2, 0.175-self.lAngles[-1]/2] self.pub_joint_states.publish(joint_state) def execute_joint_trajectory(self, goal): for point in goal.trajectory.points: print goal.trajectory.joint_names print point.positions - arm.to_angle(0, self.speed, -point.positions[goal.trajectory.joint_names.index(lJointNames[0])]) - arm.to_angle(1, self.speed, point.positions[goal.trajectory.joint_names.index(lJointNames[1])]) - arm.to_angle(2, self.speed, -point.positions[goal.trajectory.joint_names.index(lJointNames[2])]) - arm.to_angle(3, self.speed, -point.positions[goal.trajectory.joint_names.index(lJointNames[3])]) - arm.to_angle(4, self.speed, point.positions[goal.trajectory.joint_names.index(lJointNames[4])]) + lGoalPosOrdered = [ + point.positions[goal.trajectory.joint_names.index(lJointNames[0])], + point.positions[goal.trajectory.joint_names.index(lJointNames[1])], + point.positions[goal.trajectory.joint_names.index(lJointNames[2])], + point.positions[goal.trajectory.joint_names.index(lJointNames[3])], + point.positions[goal.trajectory.joint_names.index(lJointNames[4])], + ] + arm.to_angle(0, self.speed, -lGoalPosOrdered[0]) + arm.to_angle(1, self.speed, lGoalPosOrdered[1]) + arm.to_angle(2, self.speed, -lGoalPosOrdered[2]) + arm.to_angle(3, self.speed, -lGoalPosOrdered[3]) + arm.to_angle(4, self.speed, lGoalPosOrdered[4]) + + error = 0 + while True: + error = np.array(lGoalPosOrdered) - np.array(self.lAngles[:-1]) + if np.linalg.norm(error) < 0.01: + break + + if self._as.is_preempt_requested(): + self._as.set_preempted() + break + sleep(0.001) + self._feedback.status = GoalStatus.SUCCEEDED - self._feedback.feedback.joint_names = goal.trajectory.joint_names - self._feedback.feedback.desired = point + self._feedback.feedback.joint_names = lJointNames[:-1] + self._feedback.feedback.desired.positions = lGoalPosOrdered + self._feedback.feedback.actual.positions = self.lAngles[:-1] + self._feedback.feedback.error.positions = error self._as.publish_feedback(self._feedback.feedback) self._result.status = FollowJointTrajectoryResult.SUCCESSFUL self._as.set_succeeded(self._result.result) diff --git a/urdf/arm.urdf b/urdf/arm.urdf index f23fe71..f20e03e 100644 --- a/urdf/arm.urdf +++ b/urdf/arm.urdf @@ -85,7 +85,7 @@ - + @@ -101,7 +101,7 @@ - + @@ -109,7 +109,7 @@ - + -- 2.39.5