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))