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 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 18:53, 16 June 2016

The ROS script used for our project is given below

File:AmigoROSScriptPart1.png


File:AmigoROSScriptPart2.png


File:AmigoROSScriptPart3.png

Personal tools