Python move_simulation_to_point Examples

Example #1

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)

Example #2

File: delta_passive_move.py Project: awesomebytes/delta_robot

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)

Example #3

File: delta_robot_simul_demo_dynamixel.py Project: maivantech/delta_robot

            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

Example #4

File: delta_robot_simul.py Project: awesomebytes/delta_robot

         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) 
         

Example #5

        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