AMIGO ROS Script

The ROS script used for our project is given below

1. !/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) 

1. 1. 1. 1. 1. 1. 1. 1. 1. 1. 1. 1. 1. 1. 1. 1. 1. 1. intializing program ######################

if __name__ == '__main__':

   armTrajCopier = ArmTrajCopier()