def add_chain_joints(self, root_link, tip_link):
"""
Returns a dict with joint names as keys and sympy symbols
as values for all 1-dof movable robot joints in URDF between
ROOT_LINK and TIP_LINK.
:param root_link: str, denoting the root of the kin. chain
:param tip_link: str, denoting the tip of the kin. chain
:return: dict{str, sympy.Symbol}, with symbols for all joints in chain
"""
jointsAndLinks = self.urdf_robot.get_chain(root_link, tip_link, True, True, True)
parentFrame = self.frames[root_link]
for i in range(1, len(jointsAndLinks), 2):
joint_name = jointsAndLinks[i]
link_name = jointsAndLinks[i + 1]
joint = self.urdf_robot.joint_map[joint_name]
rpy = (0,0,0)
xyz = (0,0,0)
axis = (1,0,0)
if joint_name not in self._joints:
if joint.origin is not None:
rpy = joint.origin.rpy if joint.origin.rpy is not None else (0,0,0)
xyz = joint.origin.xyz if joint.origin.xyz is not None else (0,0,0)
if joint.axis != None:
axis = (joint.axis[0], joint.axis[1], joint.axis[2])
offset_frame = spw.frame3_rpy(*rpy, loc=spw.point3(*xyz))
if joint.type == 'revolute' or joint.type == 'continuous':
self._joints[joint_name] = Joint(spw.Symbol(joint_name),
joint.limit.velocity,
joint.limit.lower,
joint.limit.upper,
joint.type == 'continuous')
self.frames[link_name] = parentFrame * offset_frame * spw.frame3_axis_angle(spw.vec3(*axis),
spw.Symbol(joint_name),
spw.point3(0,0,0))
elif joint.type == 'prismatic':
self._joints[joint_name] = Joint(spw.Symbol(joint_name),
joint.limit.velocity,
joint.limit.lower,
joint.limit.upper,
False)
self.frames[link_name] = parentFrame * spw.frame3_rpy(*rpy,
loc=spw.point3(*xyz) + spw.vec3(
*axis) * spw.Symbol(joint_name))
elif joint.type == 'fixed':
self.frames[link_name] = parentFrame * spw.frame3_rpy(*rpy, loc=spw.point3(*xyz))
else:
raise Exception('Joint type "' + joint.type + '" is not supported by urdf parser.')
parentFrame = self.frames[link_name]
def test_speed_up_max(self, f1, f2):
f1_s = spw.Symbol('f1')
f2_s = spw.Symbol('f2')
expr = spw.diffable_max_fast(f1_s, f2_s)
llvm = spw.speed_up(spw.Matrix([expr]), expr.free_symbols)
kwargs = {'f1': f1, 'f2': f2}
r1_llvm = llvm(**kwargs)[0]
# r1_expr = np.float(expr.subs())
r1 = np.float(spw.diffable_max_fast(f1, f2))
self.assertTrue(np.isclose(r1, r1_llvm), msg='max({},{})={}, max_expr({},{})={}'.format(f1, f2, r1,
f1, f2, r1_llvm))
def test_speed_up_if_greater_zero(self, condition, if_result, else_result):
condition_s = spw.Symbol('condition')
if_s = spw.Symbol('if')
else_s = spw.Symbol('else')
expr = spw.diffable_if_greater_zero(condition_s, if_s, else_s)
llvm = spw.speed_up(spw.Matrix([expr]), expr.free_symbols)
kwargs = {'condition': condition,
'if': if_result,
'else': else_result}
# r1_expr = float(expr.subs(kwargs))
r1_llvm = llvm(**kwargs)[0][0]
r1 = float(spw.diffable_if_greater_zero(condition, if_result, else_result))
self.assertTrue(np.isclose(r1, r1_llvm), msg='{} if {} > 0 else {} => {} != {}'.format(if_result, condition,
else_result,
r1_llvm, r1))
def test_speed_sign(self, f1):
f1_s = spw.Symbol('f1')
expr = spw.diffable_sign(f1_s)
llvm = spw.speed_up(spw.Matrix([expr]), list(expr.free_symbols))
kwargs = {'f1': f1}
r1_llvm = llvm(**kwargs)
r1 = float(spw.diffable_sign(f1))
self.assertTrue(np.isclose(r1, r1_llvm), msg='spw.sign({})={} != np.sign({})={}'.format(f1, r1, f1, r1_llvm))
def test_speed_up_matrix_from_axis_angle(self, axis, angle):
axis_s = spw.var('x y z')
angle_s = spw.Symbol('angle')
kwargs = {'x': axis[0],
'y': axis[1],
'z': axis[2],
'angle': angle}
expr = spw.rotation_matrix_from_axis_angle(spw.Matrix(axis_s), angle_s)
llvm = spw.speed_up(spw.Matrix([expr]), expr.free_symbols)
r1_llvm = llvm(**kwargs)
r1 = np.array(spw.rotation_matrix_from_axis_angle(axis, angle)).astype(float)
self.assertTrue(np.isclose(r1, r1_llvm).all(), msg='{} {}\n{} != \n{}'.format(axis, angle, r1, r1_llvm))
def __init__(self, float_names, prefix='', suffix=''):
if prefix == '':
self._prefix = prefix
else:
self._prefix = '{}{}'.format(prefix, self.separator)
if suffix == '':
self._suffix = suffix
else:
self._suffix = '{}{}'.format(self.separator, suffix)
self._symbol_map = OrderedDict(
(fn, spw.Symbol(('{}{}{}'.format(self._prefix, fn, self._suffix))))
for fn in float_names)
self._str_map = OrderedDict(
(k, str(v)) for k, v in self._symbol_map.items())
def test_speed_up_slerp(self, q1, q2, t):
q1_s = spw.var('q1x q1y q1z q1w')
q2_s = spw.var('q2x q2y q2z q2w')
t_s = spw.Symbol('t')
q1_expr = spw.Matrix(q1_s)
q2_expr = spw.Matrix(q2_s)
expr = spw.diffable_slerp(q1_expr, q2_expr, t_s)
slerp = spw.speed_up(expr, expr.free_symbols)
kwargs = {'q1x': q1[0], 'q1y': q1[1], 'q1z': q1[2], 'q1w': q1[3],
'q2x': q2[0], 'q2y': q2[1], 'q2z': q2[2], 'q2w': q2[3],
't': t}
r1 = slerp(**kwargs).T[0]
r2 = np.array([float(x.evalf(real=True)) for x in spw.diffable_slerp(spw.Matrix(q1), spw.Matrix(q2), t)])
self.assertTrue(np.isclose(r1, r2, atol=1e-3).all() or
np.isclose(r1, -r2, atol=1e-3).all(),
msg='q1={} q2={} t={}\n{} != {}'.format(q1, q2, t, r1, r2))
def __init__(self,
joint_constraints_dict,
hard_constraints_dict,
soft_constraints_dict,
backend=None,
logging=print_wrapper):
self.backend = backend
self.logging = logging
self.joint_constraints_dict = joint_constraints_dict
self.hard_constraints_dict = hard_constraints_dict
self.soft_constraints_dict = soft_constraints_dict
self.controlled_joints_strs = list(self.joint_constraints_dict.keys())
self.controlled_joints = [
spw.Symbol(n) for n in self.controlled_joints_strs
]
self.soft_constraint_indices = {}
self.make_sympy_matrices()
self.qp_solver = QPSolver(self.H.shape[0], len(self.lbA))
def get_expression(self):
return spw.Matrix([spw.Symbol(x) for x in self._symbol_map.values()])
def __call__(self):
self.symbol_counter += 1
return spw.Symbol('a{}'.format(self.symbol_counter))