def objective(x):
"""
Objective function to minimize
:param x: state in configuration space
:returns: the objective cost
"""
FK_q = fk(x, link_lengths)
return np.sum(np.square(np.subtract(FK_q, p_d)))
def plot_solution(x):
"""
Plot IK solution.
:param x: solution state as a vector of joint angles
:returns: None
"""
fig = plt.figure()
ax = fig.gca(projection='3d')
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
# start position
ax.scatter(0, 0, 0, color='r', s=100)
# desired position
ax.scatter(p_d[0], p_d[1], p_d[2], color='g', s=100)
# plot robot
points1 = fk(x[:1], link_lengths[:1])
plt.plot([0, points1[0]], [0, points1[1]], [0, points1[2]], color='k')
for pp in range(1, len(x)):
points0 = fk(x[:pp], link_lengths[:pp])
points1 = fk(x[:pp+1], link_lengths[:pp+1])
plt.plot([points0[0], points1[0]],
[points0[1], points1[1]],
[points0[2], points1[2]],
color='k')
# add obstacle as sphere
ax.plot_wireframe(
*WireframeSphere(obstacle_center, obstacle_radius),
color='k',
alpha=0.5)
ax.set(xlim=(-2, 2), ylim=(-2, 2), zlim=(-2, 2))
# show result
plt.show()
def constraint1(x):
"""
Collision constraint for the first link
:param x: state in configuration space
:returns: constraint output (i.e. no collision if the return value is non-negative)
"""
p1 = np.array([0, 0, 0])
p2 = fk(x[:1], link_lengths[:1])
collision = line_sphere_intersection(p1,
p2,
c=obstacle_center,
r=obstacle_radius)
return -collision
def objective(x):
"""
Objective function that we want to minimize.
:param x: state in configuration space, as numpy array with dtype
np.float64
:returns: a scalar value with type np.float64 representing the objective
cost for x
"""
# FILL in your code here
s = 0
n = fk(x, link_lengths)
s = p_d - n
c = np.power(np.dot(s, s), 0.5) / 2
return c
def constraint1(x):
"""
Collision constraint for the first link.
As an inequality constraint, the constraint is satisfied (meaning there is
no collision) if the return value is non-negative.
:param x: state in configuration space, as numpy array with dtype
np.float64
:returns: constraint output as a scalar value of type np.float64
"""
# FILL in your code here
line_start = np.array([0, 0, 0])
line_end = fk(np.array([x[0]]), np.array([link_lengths[0]]))
d = line_sphere_intersection(line_start, line_end, obstacle_center, obstacle_radius)
return d
def objective(x): """ Objective function that we want to minimize. :param x: state in configuration space, as numpy array with dtype np.float64 :returns: a scalar value with type np.float64 representing the objective cost for x """ # FILL in your code here p_d = np.array([0.1, 1.33, 0]) expected=fk(x,link_lengths) val=np.float64(np.sum([(expected - p_d)**2 for p_d, expected in zip(p_d,expected)])) return val
# Uses python3
from fk import fk
def test(name, actual, expected):
actual = "{:.4f}".format(actual)
print(name, " : ", actual == expected, "[ ", actual, " : ", expected, " ]")
test("Sample 1", fk(50, [20, 50, 30], [60, 100, 120]), "180.0000")
test("Sample 2", fk(10, [30], [500]), "166.6667")
test("Case 2 of 13", fk(1000, [30], [500]), "500.0000")
def distance(angles, targetEndPoint):
currentEndPoint = np.array(fk(angles)[:, 8])
result = np.subtract(targetEndPoint, currentEndPoint)
return np.append(result, [0, 0, 0, 0])