def create_5R():
architecture = [
-22.24157649, 0.2888078, 22.51328886, -0.15281802, 17.58176891,
17.80539455, 17.51237713, 17.59555565
]
r5 = robot.FiveBars(architecture, mode=0, seed=1)
# r5 = robot.FiveBars(architecture,mode=1,seed=1)
return r5
#!/usr/bin/env python
# -*- coding: utf-8 -*-
#######################################################
# FiveBars robot
import math
import numpy
##################################################
# Definition of a 5R robot with approximate architecture
import robot
nominal_architecture = [-22.5, 0, 22.5, 0, 17.8, 17.8, 17.8, 17.8]
r5 = robot.FiveBars(nominal_architecture, seed=3, mode=0)
#~ Suite à la calibration, on a obtenu l'architecture suivante :
calibrated_architecture = [
-22.48910557, 0.2487764, 22.31083019, 0.19295762, 17.75206524, 17.75148665,
17.82798783, 18.18320809
]
# RRRRR kinematic functions
def f_RRRRR(architecture, pose, command):
[a11, a12, a21, a22, l1, l2, l3, l4] = architecture
[x1, x2] = pose
[q1, q2] = numpy.radians(command)
f1 = (a11 + l1 * math.cos(q1) - x1)**2 + (a12 + l1 * math.sin(q1) -
x2)**2 - l2**2
f2 = (a21 + l4 * math.cos(q2) - x1)**2 + (a22 + l4 * math.sin(q2) -
x2)**2 - l3**2
return [f1, f2]
print(' result : ', sol.x)
return sol.x
def f_5R(architecture, pose, command):
[o11, o12, o21, o22, l1, l2, l3, l4] = architecture
[x1, x2] = pose
[q1, q2] = numpy.radians(command)
f1 = numpy.square((o11 + numpy.cos(q1) * l1 - x1)) + numpy.square(
(o12 + numpy.sin(q1) * l1 - x2)) - numpy.square(l2)
f2 = numpy.square((o21 + numpy.cos(q2) * l4 - x1)) + numpy.square(
(o22 + numpy.sin(q2) * l4 - x2)) - numpy.square(l3)
return [f1, f2]
r = robot.FiveBars([-22.5, 0, 22.5, 0, 17.8, 17.8, 17.8, 17.8], 0, 1)
nominal_architecture = [-22.5, 0, 22.5, 0, 17.8, 17.8, 17.8, 17.8]
r.pen_up()
r.go_home()
#commandes pour le calibrage
commands = []
commands = commands + [[0, q] for q in range(80, 220, 20)
] + [[q, 180] for q in range(-40, 110, 20)]
commands = commands + [[40, q] for q in range(80, 220, 20)
] + [[q, 140] for q in range(-40, 110, 20)]
commands = commands + [[q, 180 - q] for q in range(-40, 80, 10)]
#commandes pour le test
commands2 = []
commands2 = commands2 + [[10, q] for q in range(80, 220, 10)