def test_jacobian(self):
a = w.Symbol('a')
b = w.Symbol('b')
m = w.Matrix([a + b, a**2, b**2])
jac = w.jacobian(m, [a, b])
expected = w.Matrix([[1, 1], [2 * a, 0], [0, 2 * b]])
for i in range(expected.shape[0]):
for j in range(expected.shape[1]):
assert w.equivalent(jac[i, j], expected[i, j])
def to_symbol(self, identifier):
"""
All registered identifiers will be included in self.get_symbol_map().
:type identifier: list
:return: the symbol corresponding to the identifier
:rtype: sw.Symbol
"""
assert isinstance(identifier, list) or isinstance(identifier, tuple)
identifier = tuple(identifier)
identifier_parts = identifier
if identifier not in self.key_to_expr:
expr = w.Symbol(
self.expr_separator.join([str(x) for x in identifier]))
if expr in self.expr_to_key:
raise Exception(u'{} not allowed in key'.format(
self.expr_separator))
self.key_to_expr[identifier] = expr
self.expr_to_key[str(expr)] = identifier_parts
return self.key_to_expr[identifier]
def __init__(self, urdf, base_pose=None, controlled_joints=None, path_to_data_folder=u'', *args, **kwargs):
"""
:param urdf:
:type urdf: str
:param joints_to_symbols_map: maps urdfs joint names to symbols
:type joints_to_symbols_map: dict
:param joint_vel_limit: all velocity limits which are undefined or higher than this will be set to this
:type joint_vel_limit: Symbol
"""
self._fk_expressions = {}
self._fks = {}
self._evaluated_fks = {}
self._joint_to_frame = {}
self._joint_position_symbols = KeyDefaultDict(lambda x: w.Symbol(x)) # don't iterate over this map!!
self._joint_velocity_symbols = KeyDefaultDict(lambda x: 0) # don't iterate over this map!!
self._joint_velocity_linear_limit = KeyDefaultDict(lambda x: 10000) # don't overwrite urdf limits by default
self._joint_velocity_angular_limit = KeyDefaultDict(lambda x: 100000)
self._joint_acc_linear_limit = defaultdict(lambda: 100) # no acceleration limit per default
self._joint_acc_angular_limit = defaultdict(lambda: 100) # no acceleration limit per default
self._joint_weights = defaultdict(lambda: 0)
super(Robot, self).__init__(urdf, base_pose, controlled_joints, path_to_data_folder, *args, **kwargs)
self.reinitialize()
def test_is_matrix(self):
self.assertFalse(w.is_matrix(w.Symbol('a')))
self.assertTrue(w.is_matrix(w.Matrix([[0, 0]])))
def update(self):
# TODO make this interruptable
# TODO try catch everything
move_cmd = self.get_god_map().get_data(
identifier.next_move_goal) # type: MoveCmd
if not move_cmd:
return Status.FAILURE
self.get_god_map().set_data(identifier.constraints_identifier, {})
self.get_robot()._create_constraints(self.get_god_map())
self.soft_constraints = {}
if not (self.get_god_map().get_data(identifier.check_reachability)):
self.get_god_map().set_data(identifier.maximum_collision_threshold,
0)
self.add_collision_avoidance_soft_constraints(move_cmd.collisions)
try:
self.parse_constraints(move_cmd)
except AttributeError:
self.raise_to_blackboard(
InvalidGoalException(u'couldn\'t transform goal'))
traceback.print_exc()
return Status.SUCCESS
except Exception as e:
self.raise_to_blackboard(e)
traceback.print_exc()
return Status.SUCCESS
self.get_god_map().set_data(identifier.collision_goal,
move_cmd.collisions)
self.get_god_map().set_data(identifier.soft_constraint_identifier,
self.soft_constraints)
self.get_blackboard().runtime = time()
controlled_joints = self.get_robot().controlled_joints
if (self.get_god_map().get_data(identifier.check_reachability)):
from giskardpy import casadi_wrapper as w
joint_constraints = OrderedDict()
for i, joint_name in enumerate(controlled_joints):
lower_limit, upper_limit = self.get_robot().get_joint_limits(
joint_name)
joint_symbol = self.get_robot().get_joint_position_symbol(
joint_name)
sample_period = w.Symbol(
u'rosparam_general_options_sample_period'
) # TODO this should be a parameter
# velocity_limit = self.get_robot().get_joint_velocity_limit_expr(joint_name) * sample_period
if self.get_robot().is_joint_prismatic(joint_name):
velocity_limit = self.rc_prismatic_velocity * sample_period
elif self.get_robot().is_joint_continuous(joint_name):
velocity_limit = self.rc_continuous_velocity * sample_period
elif self.get_robot().is_joint_revolute(joint_name):
velocity_limit = self.rc_revolute_velocity * sample_period
else:
velocity_limit = self.rc_other_velocity * sample_period
weight = self.get_robot()._joint_weights[joint_name]
weight = weight * (1. / (self.rc_prismatic_velocity))**2
if not self.get_robot().is_joint_continuous(joint_name):
joint_constraints[(self.get_robot().get_name(),
joint_name)] = JointConstraint(
lower=w.Max(
-velocity_limit,
lower_limit - joint_symbol),
upper=w.Min(
velocity_limit,
upper_limit - joint_symbol),
weight=weight,
linear_weight=0)
else:
joint_constraints[(self.get_robot().get_name(),
joint_name)] = JointConstraint(
lower=-velocity_limit,
upper=velocity_limit,
weight=weight,
linear_weight=0)
else:
joint_constraints = OrderedDict(
((self.robot.get_name(), k), self.robot._joint_constraints[k])
for k in controlled_joints)
hard_constraints = OrderedDict(
((self.robot.get_name(), k), self.robot._hard_constraints[k])
for k in controlled_joints if k in self.robot._hard_constraints)
self.get_god_map().set_data(identifier.joint_constraint_identifier,
joint_constraints)
self.get_god_map().set_data(identifier.hard_constraint_identifier,
hard_constraints)
return Status.SUCCESS