AMIGO ROS Script

From Control Systems Technology Group

(Difference between revisions)
Jump to: navigation, 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
 
-
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 13:45, 16 June 2016

The ROS script used for our project is given below

Personal tools