def initialise(self):
if self.goal is None:
self.goal = self.pop_goal() # type: MoveGoal
self.traj = []
if len(self.goal.cmd_seq) == 0:
self.raise_to_blackboard(InsolvableException(u'goal empty'))
if self.goal.type not in [MoveGoal.PLAN_AND_EXECUTE, MoveGoal.PLAN_ONLY]:
self.raise_to_blackboard(
InsolvableException(u'invalid move action goal type: {}'.format(self.goal.type)))
self.get_god_map().safe_set_data(identifier.execute, self.goal.type == MoveGoal.PLAN_AND_EXECUTE)
def parse_constraints(self, cmd):
"""
:type cmd: MoveCmd
:rtype: dict
"""
for constraint in itertools.chain(cmd.constraints,
cmd.joint_constraints,
cmd.cartesian_constraints):
if constraint.type not in allowed_constraint_names():
# TODO test me
raise InsolvableException(u'unknown constraint')
try:
C = eval(u'giskardpy.constraints.{}'.format(constraint.type))
except NameError as e:
# TODO return next best constraint type
raise ImplementationException(u'unsupported constraint type')
try:
if hasattr(constraint, u'parameter_value_pair'):
params = json.loads(constraint.parameter_value_pair)
else:
params = convert_ros_message_to_dictionary(constraint)
del params[u'type']
c = C(self.god_map, **params)
soft_constraints = c.get_constraint()
self.soft_constraints.update(soft_constraints)
except TypeError as e:
traceback.print_exc()
raise ImplementationException(help(c.get_constraint))
def update(self):
latest_points = self.get_god_map().safe_get_data(identifier.joint_states)
for key in latest_points:
self.joint_dict[key].append(latest_points[key].velocity)
if len(self.joint_dict.values()[0]) < self.num_points_in_fft:
return Status.RUNNING
if len(self.joint_dict.values()[0]) > self.num_points_in_fft:
for val in self.joint_dict.values():
del (val[0])
# remove joints that arent moving
joints_filtered = np.array(self.joint_dict.values())
joints_filtered = [i for i in joints_filtered if
np.any(np.abs(i) > 0.1)]
if len(joints_filtered) == 0:
return Status.RUNNING
freq = np.fft.rfftfreq(self.num_points_in_fft, d=self.sample_period)
# find index in frequency list where frequency >= min_wiggle_frequency
freq_idx = next(i for i,v in enumerate(freq) if v >= self.min_wiggle_frequency)
fft = np.fft.rfft(joints_filtered, axis=1)
fft = [np.abs(i.real) for i in fft]
for j in fft:
if np.any(j[freq_idx:] > self.wiggle_detection_threshold):
raise InsolvableException(u'endless wiggling detected')
return Status.RUNNING
def update(self):
current_js = self.god_map.get_data([self.joint_state_identifier])
time = self.god_map.get_data([self.time_identifier])
trajectory = self.god_map.get_data([self.trajectory_identifier])
# traj_length = self.god_map.get_data([self.goal_identifier, 'max_trajectory_length'])
rounded_js = self.round_js(current_js)
if trajectory is None:
trajectory = Trajectory()
trajectory.set(time, current_js)
self.god_map.set_data([self.trajectory_identifier], trajectory)
if self.is_preempted():
print(u'goal preempted')
self.stop_universe = True
return
if time >= 1:
if time > self.max_traj_length:
self.stop_universe = True
raise SolverTimeoutError(
u'didn\'t a solution after {} s'.format(
self.max_traj_length))
if np.abs([v.velocity for v in current_js.values()]).max() < self.precision or \
(self.plot and time > self.max_traj_length):
print(u'done')
if self.plot:
plot_trajectory(
trajectory,
set(
self.god_map.get_data(
[self.controlled_joints_identifier])))
self.stop_universe = True
return
if not self.plot and (rounded_js in self.past_joint_states):
self.stop_universe = True
raise InsolvableException(u'endless wiggling detected')
if time >= self.collision_time_threshold:
cp = self.god_map.get_data([self.closest_point_identifier])
if closest_point_constraint_violated(cp, tolerance=1):
self.stop_universe = True
raise PathCollisionException(
u'robot is in collision after {} seconds'.format(
self.collision_time_threshold))
self.past_joint_states.add(rounded_js)
def update(self):
# TODO make this interruptable
# TODO try catch everything
move_cmd = self.get_god_map().safe_get_data(
identifier.next_move_goal) # type: MoveCmd
if not move_cmd:
return Status.FAILURE
self.get_god_map().safe_set_data(identifier.constraints_identifier, {})
if self.has_robot_changed():
self.soft_constraints = {}
# TODO split soft contraints into js, coll and cart; update cart always and js/coll only when urdf changed, js maybe never
self.add_js_controller_soft_constraints()
self.add_collision_avoidance_soft_constraints()
try:
self.parse_constraints(move_cmd)
except AttributeError:
self.raise_to_blackboard(
InsolvableException(u'couldn\'t transform goal'))
traceback.print_exc()
return Status.SUCCESS
except InsolvableException as e:
self.raise_to_blackboard(e)
traceback.print_exc()
return Status.SUCCESS
except Exception as e:
self.raise_to_blackboard(e)
traceback.print_exc()
return Status.SUCCESS
# self.set_unused_joint_goals_to_current()
self.get_god_map().safe_set_data(identifier.collision_goal_identifier,
move_cmd.collisions)
self.get_god_map().safe_set_data(identifier.soft_constraint_identifier,
self.soft_constraints)
self.get_blackboard().runtime = time()
return Status.SUCCESS