AMIGO ROS Script

From Control Systems Technology Group

(Difference between revisions)
Jump to: navigation, search
(Created page with 'The ROS script used for our project is given below #!/usr/bin/python import rospy # Standaard tue importeer shizzle import smach import random import argparse import sys from…')
Line 5: Line 5:
import rospy
import rospy
-
 
-
# Standaard tue importeer shizzle
 
import smach
import smach
import random
import random
Line 13: Line 11:
from robot_smach_states.util.startup import startup
from robot_smach_states.util.startup import startup
-
 
-
# het type bericht dat gecommuniceerd wordt
 
from std_msgs.msg import Float64MultiArray
from std_msgs.msg import Float64MultiArray
Line 47: Line 43:
         # blijf doorgaan
         # blijf doorgaan
         rospy.spin()
         rospy.spin()
-
 
+
     
-
 
+
-
 
+
-
 
+
-
       
+
     def move_arms(self, multiArray):
     def move_arms(self, multiArray):
          
          

Revision as of 13:44, 16 June 2016

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. intializing program ######################

if __name__ == '__main__':

   armTrajCopier = ArmTrajCopier()
Personal tools