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 File:AmigoROSScriptPart1.png File:AmigoROSScriptPart2.png File:AmigoROSScriptPart3.png')
Line 2: Line 2:


[[File:AmigoROSScriptPart1.png]]
[[File:AmigoROSScriptPart1.png]]
#!/usr/bin/python


import rospy


# Standaard tue importeer shizzle
[[File:AmigoROSScriptPart2.png]]
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:
[[File:AmigoROSScriptPart3.png]]
    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 19:53, 16 June 2016

The ROS script used for our project is given below

AmigoROSScriptPart1.png


AmigoROSScriptPart2.png


AmigoROSScriptPart3.png