import swift

import spatialgeometry as sg

import roboticstoolbox as rtb

import spatialmath as sm

import numpy as np

import qpsolvers as qp

import math

def transform_between_vectors(a, b):

# Finds the shortest rotation between two vectors using angle-axis,

# then outputs it as a 4x4 transformation matrix

a = a / np.linalg.norm(a)

b = b / np.linalg.norm(b)

angle = np.arccos(np.dot(a, b))

axis = np.cross(a, b)

return sm.SE3.AngleAxis(angle, axis)

env = swift.Swift()

env.launch()

fetch = rtb.models.Fetch()

fetch_camera = rtb.models.FetchCamera()

fetch.q = fetch.qz

fetch_camera.q = fetch_camera.qz

target = sg.Sphere(radius=0.05, base=sm.SE3(-2.0, 0.0, 0.5))

sight_base = sm.SE3.Ry(np.pi / 2) * sm.SE3(0.0, 0.0, 2.5)

centroid_sight = sg.Cylinder(

radius=0.001,

length=5.0,

base=fetch_camera.fkine(fetch_camera.q, fast=True) @ sight_base.A,

)

env.add(fetch)

env.add(fetch_camera)

env.add(centroid_sight)

env.add(target)

Tep = fetch.fkine(fetch.q)

Tep.A[:3, :3] = sm.SE3.Rz(np.pi).R

Tep.A[:3, 3] = target.base.t

env.step()

n_base = 2

n_arm = 8

n_camera = 2

n = n_base + n_arm + n_camera

def step():

# Find end-effector pose in world frame

wTe = fetch.fkine(fetch.q, fast=True)

# Find camera pose in world frame

wTc = fetch_camera.fkine(fetch_camera.q, fast=True)

# Find transform between end-effector and goal

eTep = np.linalg.inv(wTe) @ Tep.A

# Find transform between camera and goal

cTep = np.linalg.inv(wTc) @ Tep.A

# Spatial error between end-effector and target

et = np.sum(np.abs(eTep[:3, -1]))

# Weighting function used for objective function

def w_lambda(et, alpha, gamma):

return alpha * np.power(et, gamma)

# Quadratic component of objective function

Q = np.eye(n + 10)

Q[: n_base + n_arm, : n_base + n_arm] *= 0.01 # Robotic manipulator

Q[:n_base, :n_base] *= w_lambda(et, 1.0, -1.0) # Mobile base

Q[n_base + n_arm : n, n_base + n_arm : n] *= 0.01 # Camera

Q[n : n + 3, n : n + 3] *= w_lambda(et, 1000.0, -2.0) # Slack arm linear

Q[n + 3 : n + 6, n + 3 : n + 6] *= w_lambda(et, 0.01, -5.0) # Slack arm angular

Q[n + 6:-1, n + 6:-1] *= 100 # Slack camera

Q[-1, -1] *= w_lambda(et, 1000.0, 3.0) # Slack self-occlusion

# Calculate target velocities for end-effector to reach target

v_manip, _ = rtb.p_servo(wTe, Tep, 1.5)

v_manip[3:] *= 1.3

# Calculate target angular velocity for camera to rotate towards target

head_rotation = transform_between_vectors(

np.array([1, 0, 0]), cTep[:3, 3]

)

v_camera, _ = rtb.p_servo(sm.SE3(), head_rotation, 25)

# The equality contraints to achieve velocity targets

Aeq = np.c_[

fetch.jacobe(fetch.q, fast=True),

np.zeros((6, 2)),

np.eye(6),

np.zeros((6, 4))

]

beq = v_manip.reshape((6,))

jacobe_cam = fetch_camera.jacobe(fetch_camera.q, fast=True)[3:, :]

Aeq_cam = np.c_[

jacobe_cam[:, :3],

np.zeros((3, 7)),

jacobe_cam[:, 3:],

np.zeros((3, 6)),

np.eye(3),

np.zeros((3, 1)),

]

Aeq = np.r_[Aeq, Aeq_cam]

beq = np.r_[beq, v_camera[3:].reshape((3,))]

# The inequality constraints for joint limit avoidance

Ain = np.zeros((n + 10, n + 10))

bin = np.zeros(n + 10)

# The minimum angle (in radians) in which the joint is allowed to approach

# to its limit

ps = 0.1

# The influence angle (in radians) in which the velocity damper

# becomes active

pi = 0.9

# Form the joint limit velocity damper

Ain[: fetch.n, : fetch.n], bin[: fetch.n] = fetch.joint_velocity_damper(

ps, pi, fetch.n

)

Ain_torso, bin_torso = fetch_camera.joint_velocity_damper(0.0, 0.05, fetch_camera.n)

Ain[2, 2] = Ain_torso[2, 2]

bin[2] = bin_torso[2]

Ain_cam, bin_cam = fetch_camera.joint_velocity_damper(ps, pi, fetch_camera.n)

Ain[n_base + n_arm : n, n_base + n_arm : n] = Ain_cam[3:, 3:]

bin[n_base + n_arm : n] = bin_cam[3:]

# Create line of sight object between camera and object

c_Ain, c_bin = fetch.vision_collision_damper(

target,

camera=fetch_camera,

camera_n=2,

q=fetch.q[: fetch.n],

di=0.3,

ds=0.2,

xi=1.0,

end=fetch.link_dict["gripper_link"],

start=fetch.link_dict["shoulder_pan_link"],

)

if c_Ain is not None and c_bin is not None:

c_Ain = np.c_[c_Ain, np.zeros((c_Ain.shape[0], 9)), -np.ones((c_Ain.shape[0], 1))]

Ain = np.r_[Ain, c_Ain]

bin = np.r_[bin, c_bin]

# Linear component of objective function: the manipulability Jacobian

c = np.concatenate(

(

np.zeros(n_base),

-fetch.jacobm(start=fetch.links[3]).reshape((n_arm,)),

np.zeros(n_camera),

np.zeros(10),

)

)

# Get base to face end-effector

kε = 0.5

bTe = fetch.fkine(fetch.q, include_base=False, fast=True)

θε = math.atan2(bTe[1, -1], bTe[0, -1])

ε = kε * θε

c[0] = -ε

# The lower and upper bounds on the joint velocity and slack variable

lb = -np.r_[

fetch.qdlim[: fetch.n],

fetch_camera.qdlim[3 : fetch_camera.n],

100 * np.ones(9),

0,

]

ub = np.r_[

fetch.qdlim[: fetch.n],

fetch_camera.qdlim[3 : fetch_camera.n],

100 * np.ones(9),

100,

]

# Solve for the joint velocities dq

qd = qp.solve_qp(Q, c, Ain, bin, Aeq, beq, lb=lb, ub=ub)

qd_cam = np.concatenate((qd[:3], qd[fetch.n : fetch.n + 2]))

qd = qd[: fetch.n]

if et > 0.5:

qd *= 0.7 / et

qd_cam *= 0.7 / et

else:

qd *= 1.4

qd_cam *= 1.4

arrived = et < 0.02

fetch.qd = qd

fetch_camera.qd = qd_cam

centroid_sight._base = fetch_camera.fkine(fetch_camera.q, fast=True) @ sight_base.A

return arrived

arrived = False

while not arrived:

arrived = step()

env.step(0.01)

"qr", np.array([0, 0, 0.05, 1.32, 1.4, -0.2, 1.72, 0, 1.66, 0])