def add_link(self,
a=0,
alpha=0,
d=None,
theta=None,
mass=0,
com=np.zeros(3),
inertia=np.zeros((3, 3))):
self.links = self.links + (links.Link(a, alpha, d, theta, mass, com,
inertia), )
T_to_parent = self.links[-1].T_to_parent(0)
rotation = T_to_parent[:3, :3]
translation = T_to_parent[:3, 3]
if theta is None:
joint_type = Rbdl.JointTypeRevolute
elif d is None:
joint_type = Rbdl.JointTypePrismatic
joint_axis = np.array([0, 0, 1])
com = self.links[-1].com
inertia = self.links[-1].inertia
self.rbdl.add_link(rotation, translation, joint_type, joint_axis, mass,
com, inertia)
def visit():
"""
Endpoint for adding new visited links
:return: status
"""
data = request.get_json()
links_list = [links.Link(url) for url in data['links']]
app_ctx.links.visit(links_list)
return success_response()
def decode_links(encoded_links):
def decode_var(var):
return float(var) if var != "" else None
encoded_links = [[decode_var(el) for el in link.split(",")]
for link in encoded_links.split(";")]
arr_links = []
for l in encoded_links:
try:
arr_links.append(links.Link(l[0], l[1], l[2], l[3]))
except Exception as err:
break
if not arr_links:
raise RuntimeError("Could not parse dh_table.")
return arr_links
def jacobian_app():
import redis
import numpy as np
import time
from util import redis_encode, redis_decode
import rotations
import links
import jacobian
global args
KEY_PREFIX = "cs223a::jacobian::"
KEY_SET = KEY_PREFIX + "set"
KEY_Q = KEY_PREFIX + "q"
KEY_DH_TABLE = KEY_PREFIX + "dh_table"
KEY_EE_OFFSET = KEY_PREFIX + "ee_offset"
KEY_JV = KEY_PREFIX + "J_v"
KEY_JW = KEY_PREFIX + "J_w"
KEY_ROBOT = KEY_PREFIX + "robot"
redis_db = redis.Redis(host=args.redis_host,
port=args.redis_port,
db=args.redis_db,
decode_responses=True)
def encode_links(arr_links):
def encode_var(var):
return var if var is not None else ""
encoded_links = [
"{0},{1},{2},{3}".format(l.a, l.alpha, encode_var(l.d),
encode_var(l.theta)) for l in arr_links
]
encoded_links += [",,,"]
encoded_links = ";".join(encoded_links)
return encoded_links
def decode_links(encoded_links):
def decode_var(var):
return float(var) if var != "" else None
encoded_links = [[decode_var(el) for el in link.split(",")]
for link in encoded_links.split(";")]
arr_links = []
for l in encoded_links:
try:
arr_links.append(links.Link(l[0], l[1], l[2], l[3]))
except Exception as err:
break
if not arr_links:
raise RuntimeError("Could not parse dh_table.")
return arr_links
def encode_robot(arr_links, q, ee_offset=None):
import json
if ee_offset is None:
ee_offset = np.zeros((3, ))
Ts = links.T_all_to_prev(arr_links, q)
quaternions = [rotations.mat_to_quat(T[:3, :3]).array() for T in Ts]
positions = [T[:3, 3] for T in Ts]
joint_types = links.prismatic_joints(arr_links)
json_robot = {
"links": [{"quat": q.tolist(),
"pos" : p.tolist(),
"type": t} \
for q, p, t in zip(quaternions, positions, joint_types)],
"ee": {
"pos": ee_offset.tolist()
}
}
return json.dumps(json_robot)
arr_links_0 = [
links.Link(0, 0, 0, None),
links.Link(0.1, np.pi / 2, 0, None),
links.Link(0, -np.pi / 2, None, np.pi / 2),
links.Link(0, np.pi / 2, 0.1, None)
]
arr_links = arr_links_0
q = np.array([0, 0, 0.2, 0])
ee_offset = np.array([0.2, 0, 0])
redis_db.mset({
KEY_SET:
"",
KEY_Q:
redis_encode(q),
KEY_DH_TABLE:
encode_links(arr_links),
KEY_EE_OFFSET:
redis_encode(ee_offset),
KEY_JV:
redis_encode(
jacobian.linear_jacobian(arr_links, q, pos_in_link=ee_offset)),
KEY_JW:
redis_encode(jacobian.angular_jacobian(arr_links, q)),
KEY_ROBOT:
encode_robot(arr_links, q, ee_offset=ee_offset)
})
while True:
time.sleep(1)
try:
val_set = redis_db.get(KEY_SET)
if val_set == KEY_Q:
q = redis_decode(redis_db.get(KEY_Q))
elif val_set == KEY_DH_TABLE:
arr_links = decode_links(redis_db.get(KEY_DH_TABLE))
n = len(arr_links)
if n < q.shape[0]:
q = q[:n]
elif n > q.shape[0]:
q_old = q
q = np.zeros((n, ))
q[:q_old.shape[0]] = q_old
elif val_set == KEY_EE_OFFSET:
ee_offset = redis_decode(redis_db.get(KEY_EE_OFFSET))
else:
continue
redis_db.mset({
KEY_SET:
"",
KEY_Q:
redis_encode(q),
KEY_DH_TABLE:
encode_links(arr_links),
KEY_EE_OFFSET:
redis_encode(ee_offset),
KEY_JV:
redis_encode(
jacobian.linear_jacobian(arr_links,
q,
pos_in_link=ee_offset)),
KEY_JW:
redis_encode(jacobian.angular_jacobian(arr_links, q)),
KEY_ROBOT:
encode_robot(arr_links, q, ee_offset=ee_offset)
})
except Exception as err:
print(err)
arr_links = arr_links_0
q = np.array([0, 0, 0.2, 0])
ee_offset = np.array([0.2, 0, 0])
redis_db.mset({
KEY_SET:
"",
KEY_Q:
redis_encode(q),
KEY_DH_TABLE:
encode_links(arr_links),
KEY_EE_OFFSET:
redis_encode(ee_offset),
KEY_JV:
redis_encode(
jacobian.linear_jacobian(arr_links,
q,
pos_in_link=ee_offset)),
KEY_JW:
redis_encode(jacobian.angular_jacobian(arr_links, q)),
KEY_ROBOT:
encode_robot(arr_links, q, ee_offset=ee_offset)
})
def dh_app():
import redis
import numpy as np
import time
from util import redis_encode, redis_decode
import rotations
import links
global args
KEY_PREFIX = "cs223a::dh::"
KEY_SET = KEY_PREFIX + "set"
KEY_Q = KEY_PREFIX + "q"
KEY_T_EE_TO_0 = KEY_PREFIX + "T_ee_to_0"
KEY_DH_TABLE = KEY_PREFIX + "dh_table"
KEY_ROBOT = KEY_PREFIX + "robot"
redis_db = redis.Redis(host=args.redis_host,
port=args.redis_port,
db=args.redis_db,
decode_responses=True)
def encode_links(arr_links):
def encode_var(var):
return var if var is not None else ""
encoded_links = [
"{0},{1},{2},{3}".format(l.a, l.alpha, encode_var(l.d),
encode_var(l.theta)) for l in arr_links
]
encoded_links += [",,,"]
encoded_links = ";".join(encoded_links)
return encoded_links
def decode_links(encoded_links):
def decode_var(var):
return float(var) if var != "" else None
encoded_links = [[decode_var(el) for el in link.split(",")]
for link in encoded_links.split(";")]
arr_links = []
for l in encoded_links:
try:
arr_links.append(links.Link(l[0], l[1], l[2], l[3]))
except Exception as err:
break
if not arr_links:
raise RuntimeError("Could not parse dh_table.")
return arr_links
def encode_robot(arr_links, q):
import json
Ts = links.T_all_to_prev(arr_links, q)
quaternions = [rotations.mat_to_quat(T[:3, :3]).array() for T in Ts]
positions = [T[:3, 3] for T in Ts]
joint_types = links.prismatic_joints(arr_links)
json_robot = {
"links": [{"quat": q.tolist(),
"pos" : p.tolist(),
"type": t} \
for q, p, t in zip(quaternions, positions, joint_types)]
}
return json.dumps(json_robot)
arr_links = [links.Link(0, 0, None, 0)]
q = np.zeros((1, ))
redis_db.mset({
KEY_SET: "",
KEY_Q: redis_encode(q),
KEY_T_EE_TO_0: "1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1",
KEY_DH_TABLE: encode_links(arr_links),
KEY_ROBOT: encode_robot(arr_links, q)
})
while True:
time.sleep(1)
try:
val_set = redis_db.get(KEY_SET)
if val_set == KEY_Q:
q = redis_decode(redis_db.get(KEY_Q))
T_ee_to_0 = links.T_i_to_j(arr_links, q, len(arr_links), 0)
elif val_set == KEY_DH_TABLE:
arr_links = decode_links(redis_db.get(KEY_DH_TABLE))
n = len(arr_links)
if n < q.shape[0]:
q = q[:n]
elif n > q.shape[0]:
q_old = q
q = np.zeros((n, ))
q[:q_old.shape[0]] = q_old
T_ee_to_0 = links.T_i_to_j(arr_links, q, len(arr_links), 0)
else:
continue
redis_db.mset({
KEY_SET: "",
KEY_Q: redis_encode(q),
KEY_T_EE_TO_0: redis_encode(T_ee_to_0),
KEY_DH_TABLE: encode_links(arr_links),
KEY_ROBOT: encode_robot(arr_links, q)
})
except Exception as err:
print(err)
arr_links = [links.Link(0, 0, None, 0)]
q = np.zeros((1, ))
redis_db.mset({
KEY_SET: "",
KEY_Q: redis_encode(q),
KEY_T_EE_TO_0: "1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1",
KEY_DH_TABLE: encode_links(arr_links),
KEY_ROBOT: encode_robot(arr_links, q)
})
def setUp(self):
self.componentA = components.Laser()
self.componentB = components.CavityMirror()
self.link = links.Link(self.componentA.getOutputNode('out'),
self.componentB.getInputNode('fr'), 10)