Difference between revisions of "AMIGO ROS Script"

From Control Systems Technology Group
Jump to navigation Jump to search
(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
# Standaard tue importeer shizzle
import smach
import random
import argparse
import sys
from robot_smach_states.util.startup import startup
# het type bericht dat gecommuniceerd wordt
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:50, 16 June 2016

The ROS script used for our project is given below

  1. !/usr/bin/python

import rospy

  1. Standaard tue importeer shizzle

import smach import random import argparse import sys from robot_smach_states.util.startup import startup

  1. het type bericht dat gecommuniceerd wordt

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()