AMIGO ROS Script: Difference between revisions

From Control Systems Technology Group
Jump to navigation Jump to search
No edit summary
(Replaced content with 'The ROS script used for our project is given below')
Line 1: Line 1:
The ROS script used for our project is given below
The ROS script used for our project is given below
#!/usr/bin/python
import rospy
import smach
import random
import argparse
import sys
from robot_smach_states.util.startup import startup
from std_msgs.msg import Float64MultiArray
class ArmTrajCopier:
    def __init__(self):
        rospy.init_node('armTrajCopier')
       
        # standaard opstart gedoe van de AMIGO
        parser = argparse.ArgumentParser()
        parser.add_argument('robot', help='Robot name')
        args = parser.parse_args()
        rospy.loginfo('args: %s', args)
       
        robot_name = args.robot
       
        if robot_name == 'amigo':
            from robot_skills.amigo import Amigo as Robot
        else:
            print "unknown robot"
            return 1
       
        self.robot = Robot()
       
        multiArray = Float64MultiArray()
        multiArray.data = [0.0, 1.5, 0.0, 0.0, 0.0, 0.0, 0.0,
        0.0, 1.5, 0.0, 0.0, 0.0, 0.0, 0.0]
        self.move_arms(multiArray)
       
        # begin een rostopic en subscribe hier op
        rospy.Subscriber("arm_angles", Float64MultiArray, self.move_arms)
        # blijf doorgaan
        rospy.spin()
     
    def move_arms(self, multiArray):
       
        # assign data
      left  = multiArray.data[0:7]
      right = multiArray.data[7:15]
      # clamp data
      #self.clamp(self, left[0], 0, -1.57
      max(min(left[0], 0.0 ), -1.57)
      max(min(left[1], 1.57), -1.57)
      max(min(left[2], 1.57), -1.57)
      max(min(left[3], 2.23),  0.0)
      max(min(left[4], 1.83), -1.83)
      max(min(left[5], 0.95), -0.95)
      max(min(left[6], 0.61), -0.61)
      max(min(right[0], 0.0 ), -1.57)
      max(min(right[1], 1.57), -1.57)
      max(min(right[2], 1.57), -1.57)
      max(min(right[3], 2.23),  0.0)
      max(min(right[4], 1.83), -1.83)
      max(min(right[5], 0.95), -0.95)
      max(min(right[6], 0.61), -0.61)
        # maak een nieuwe trajectories voor links en rechts
        self.robot.leftArm.default_trajectories['newTrajL']  = [    left    ]
        self.robot.rightArm.default_trajectories['newTrajR'] = [    right    ]
        # voer de trajectories uit
        self.robot.leftArm.send_joint_trajectory('newTrajL', timeout=0.0)
        self.robot.rightArm.send_joint_trajectory('newTrajR', timeout=0.0)
           
##################  intializing program  ######################   
if __name__ == '__main__':
    armTrajCopier = ArmTrajCopier()

Revision as of 14:45, 16 June 2016

The ROS script used for our project is given below

Retrieved from "https://dsdwiki.wtb.tue.nl/index.php?title=AMIGO_ROS_Script&oldid=28853"