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