fixed slerp

SemRoCo · Jul 26, 2019 · 6d8bf8e · 6d8bf8e
1 parent be39578
commit 6d8bf8e
Show file tree

Hide file tree

Showing 3 changed files with 24 additions and 20 deletions.
diff --git a/src/giskardpy/constraints.py b/src/giskardpy/constraints.py
@@ -391,8 +391,10 @@ def get_constraint(self):
 
  angle = sw.rotation_distance(current_rotation, goal_rotation)
  angle = sw.diffable_abs(angle)
- angle = sw.if_eq_zero(angle, max_speed, angle)
- capped_angle = sw.diffable_min_fast(max_speed / (gain * angle), 1)
+
+ denominator = (gain * angle)
+ denominator = sw.if_eq_zero(denominator, -1, denominator) # FIXME breaks if maxspeed or gain are negative
+ capped_angle = sw.diffable_max_fast(sw.diffable_min_fast(max_speed / denominator, 1), 0)
 
  q1 = sw.quaternion_from_matrix(current_rotation)
  q2 = sw.quaternion_from_matrix(goal_rotation)
@@ -417,15 +419,16 @@ def get_constraint(self):
  upper=r_rot_control[2],
  weight=weight,
  expression=c_aa[2])
- add_debug_constraint(soft_constraints, str(self)+'/angle', angle)
- add_debug_constraint(soft_constraints, str(self)+'/capped_angle', capped_angle)
- add_debug_constraint(soft_constraints, str(self)+'/max_speed / (gain * angle)', (gain * angle)/max_speed)
- add_debug_constraint(soft_constraints, str(self)+'/max_speed', max_speed)
- add_debug_constraint(soft_constraints, str(self)+'/gain', gain)
- add_debug_constraint(soft_constraints, str(self)+'/asdf[0]', asdf[0])
- add_debug_constraint(soft_constraints, str(self)+'/asdf[1]', asdf[1])
- add_debug_constraint(soft_constraints, str(self)+'/asdf[2]', asdf[2])
- add_debug_constraint(soft_constraints, str(self)+'/asdf[3]', asdf[3])
+ # add_debug_constraint(soft_constraints, str(self)+'/angle', angle)
+ # add_debug_constraint(soft_constraints, str(self)+'/capped_angle', capped_angle)
+ # add_debug_constraint(soft_constraints, str(self)+'/max_speed / (gain * angle)', max_speed/(gain * angle))
+ # add_debug_constraint(soft_constraints, str(self)+'/max_speed', max_speed)
+ # add_debug_constraint(soft_constraints, str(self)+'/gain', gain)
+ # add_debug_constraint(soft_constraints, str(self)+'/denominator', denominator)
+ # add_debug_constraint(soft_constraints, str(self)+'/asdf[0]', asdf[0])
+ # add_debug_constraint(soft_constraints, str(self)+'/asdf[1]', asdf[1])
+ # add_debug_constraint(soft_constraints, str(self)+'/asdf[2]', asdf[2])
+ # add_debug_constraint(soft_constraints, str(self)+'/asdf[3]', asdf[3])
  return soft_constraints
 
 

diff --git a/src/giskardpy/qp_problem_builder.py b/src/giskardpy/qp_problem_builder.py
@@ -196,15 +196,15 @@ def debug_print(self, np_H, np_A, np_lb, np_ub, np_lbA, np_ubA, xdot_full=None):
  else:
  self.lbAs = self.lbAs.T.append(p_lbA.T, ignore_index=True).T
  # self.lbAs.T[[c for c in self.lbAs.T.columns if 'dist' in c]].plot()
- arrays = [(p_weights, u'H'),
- (p_A, u'A'),
- (p_lbA, u'lbA'),
- (p_ubA, u'ubA'),
- (p_lb, u'lb'),
- (p_ub, u'ub')]
- for a, name in arrays:
- self.check_for_nan(name, a)
- self.check_for_big_numbers(name, a)
+ # arrays = [(p_weights, u'H'),
+ #  (p_A, u'A'),
+ #  (p_lbA, u'lbA'),
+ #  (p_ubA, u'ubA'),
+ #  (p_lb, u'lb'),
+ #  (p_ub, u'ub')]
+ # for a, name in arrays:
+ #  self.check_for_nan(name, a)
+ #  self.check_for_big_numbers(name, a)
  pass
 
  def check_for_nan(self, name, p_array):

diff --git a/src/giskardpy/qp_solver.py b/src/giskardpy/qp_solver.py
@@ -75,6 +75,7 @@ def solve(self, H, g, A, lb, ub, lbA, ubA, nWSR=None):
  r = 5
  A = np.round(A, r)
  nWSR = None
+ self.started = False
  else: # if not break
  self.started = False
  raise QPSolverException(self.RETURN_VALUE_DICT[success])