def callback_2(data): global motor1_pos global motor2_pos global motor3_pos global initializedMarkerArray global trailMarkerArray global current_trail_point global current_trail_point_qtty global max_trail_points # we move from 500 to 850 so we need to adapt the data (1023 is max pos) motor2_pos = math.degrees( data.current_pos) #(data.position-500.0)/ 350.0 * 90.0 #print "Motor2 in pos: %d" % (motor2_pos) posefinal = delta_calcForward(motor1_pos, motor2_pos, motor3_pos) initializedMarkerArray = move_simulation_to_point(posefinal[1], posefinal[2], posefinal[3], initializedMarkerArray) point = generate_last_visited_point(posefinal[1], posefinal[2], posefinal[3]) current_trail_point_qtty += 1 if current_trail_point_qtty >= max_trail_points: trailMarkerArray.markers.pop( 0) # the 11th element is the first testpoint always current_trail_point_qtty -= 1 trailMarkerArray.markers.append(point)
def callback_2(data): global motor1_pos global motor2_pos global motor3_pos global initializedMarkerArray global trailMarkerArray global current_trail_point global current_trail_point_qtty global max_trail_points # we move from 500 to 850 so we need to adapt the data (1023 is max pos) motor2_pos = math.degrees(data.current_pos)#(data.position-500.0)/ 350.0 * 90.0 #print "Motor2 in pos: %d" % (motor2_pos) posefinal = delta_calcForward(motor1_pos, motor2_pos, motor3_pos) initializedMarkerArray = move_simulation_to_point(posefinal[1], posefinal[2], posefinal[3], initializedMarkerArray) point = generate_last_visited_point(posefinal[1], posefinal[2], posefinal[3]) current_trail_point_qtty += 1 if current_trail_point_qtty >= max_trail_points: trailMarkerArray.markers.pop(0) # the 11th element is the first testpoint always current_trail_point_qtty -= 1 trailMarkerArray.markers.append(point)
testpoint.color.r = 1.0 testpoint.color.g = 1.0 testpoint.color.b = 1.0 testpoint.pose.orientation.w = 1.0 testpoint.pose.position.x = posefinal[1] testpoint.pose.position.y = posefinal[2] testpoint.pose.position.z = posefinal[3] print "Testpoint position:\n" + str(testpoint.pose.position) print "\n" testpoint.id = idcount idcount += 1 # Llamar a funcion que mueve los motores move_dynamixels( alpha, beta, theta) move_dynamixels(theta, theta, theta) initializedMarkerArray = move_simulation_to_point( posefinal[1], posefinal[2], posefinal[3], initializedMarkerArray ) print "Inverse for this point" angles = delta_calcInverse(posefinal[1], posefinal[2], posefinal[3]) print str(angles[1]) + ", " + str(angles[2]) + ", " + str(angles[3]) # print str(math.degrees(angles[1])) + ", "+ str(math.degrees(angles[2])) + ", "+ str(math.degrees(angles[3])) print "\n" initializedMarkerArray.markers.append(testpoint) # Publish the MarkerArray publisher.publish(initializedMarkerArray) rospy.sleep(0.02) theta1 = 1
testpoint.color.a = 1.0 testpoint.color.r = 1.0 testpoint.color.g = 1.0 testpoint.color.b = 1.0 testpoint.pose.orientation.w = 1.0 testpoint.pose.position.x = initialx testpoint.pose.position.y = initialy testpoint.pose.position.z = initialz testpoint.id = idcount idcount += 1 print "Inverse for this point" angles = delta_calcInverse(initialx, initialy, initialz) print angles if angles[0] > -1 and angles[1] > 0 and angles[2] > 0 and angles[3] > 0: initializedMarkerArray = move_simulation_to_point(initialx, initialy, initialz, initializedMarkerArray) f.write(str(initialx) + ", " + str(initialy) + ", " + str(initialz) + "\n") else: print "Pose not attainable" testpoint.color.r = 1.0 testpoint.color.g = 0.0 testpoint.color.b = 0.0 #finished = True initializedMarkerArray.markers.append(testpoint) # Publish the MarkerArray publisher.publish(initializedMarkerArray) rospy.sleep(0.02)
testpoint.color.a = 1.0 testpoint.color.r = 1.0 testpoint.color.g = 1.0 testpoint.color.b = 1.0 testpoint.pose.orientation.w = 1.0 testpoint.pose.position.x = posefinal[1] testpoint.pose.position.y = posefinal[2] testpoint.pose.position.z = posefinal[3] print "Testpoint position:\n" + str(testpoint.pose.position) print "\n" testpoint.id = idcount idcount += 1 # Llamar a funcion que mueve los motores move_dynamixels( alpha, beta, theta) move_dynamixels(angles[1], angles[2], angles[3]) initializedMarkerArray = move_simulation_to_point( posefinal[1], posefinal[2], posefinal[3], initializedMarkerArray) initializedMarkerArray.markers.append(testpoint) # Publish the MarkerArray publisher.publish(initializedMarkerArray) rospy.sleep(0.02) while not rospy.is_shutdown(): publisher.publish(initializedMarkerArray) rospy.sleep(0.02) except KeyboardInterrupt: # do nothing here f.close() pass