angle/urdf fixes

diff --git a/scripts/arm_ros_conn.py b/scripts/arm_ros_conn.py
index c4ab470bf71d21b4f110f36adba824f1b3db4fca..d11344fa429925a11b68eccccf841d8c98a2a07a 100755 (executable)
--- 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 62d8e8a5b14295fad490f5c4a17eacadf52df7ae..f23fe71a04d2b4cf17a77292d446fd2beefd37b9 100644 (file)
--- a/urdf/arm.urdf
+++ b/urdf/arm.urdf
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<robot name="wild_thumper">
+<robot name="arm">
        <link name="base_link">
        </link>