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