From d321c12780c8a802f363a723de0cac3954595938 Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Mon, 19 Oct 2015 22:19:19 +0200 Subject: [PATCH] angle/urdf fixes --- scripts/arm_ros_conn.py | 9 ++++++++- urdf/arm.urdf | 2 +- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/scripts/arm_ros_conn.py b/scripts/arm_ros_conn.py index c4ab470..d11344f 100755 --- a/scripts/arm_ros_conn.py +++ b/scripts/arm_ros_conn.py @@ -3,7 +3,9 @@ import rospy import arm +import actionlib from sensor_msgs.msg import JointState +from control_msgs.msg import FollowJointTrajectoryAction class ARMRosConn(): def __init__(self): @@ -16,6 +18,8 @@ class ARMRosConn(): arm.set_tolerance(3, 0) arm.set_tolerance(5, 0) + self._as = actionlib.SimpleActionServer("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction, execute_cb=self.execute_joint_trajectory, auto_start = False) + self._as.start() self.pub_joint_states = rospy.Publisher("joint_states", JointState, queue_size=16) self.run() @@ -29,9 +33,12 @@ class ARMRosConn(): joint_state = JointState() joint_state.header.stamp = rospy.Time.now() joint_state.name = ["base_to_link1", "link_1_2_joint", "link_2_3_joint", "gripper_joint_1", "gripper_joint_2", "left_gripper_joint", "right_gripper_joint"] - joint_state.position = [arm.get_angle(0), arm.get_angle(1), arm.get_angle(2), arm.get_angle(3), arm.get_angle(4), arm.get_angle(5)/2, arm.get_angle(5)/2] + 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.pub_joint_states.publish(joint_state) + def execute_joint_trajectory(self, goal): + print goal + if __name__ == '__main__': ARMRosConn() diff --git a/urdf/arm.urdf b/urdf/arm.urdf index 62d8e8a..f23fe71 100644 --- a/urdf/arm.urdf +++ b/urdf/arm.urdf @@ -1,5 +1,5 @@ - + -- 2.39.2