merged with constraint update

config/pr2.yaml

@@ -2,13 +2,17 @@ enable_collision_marker: True
 enable_visualization: True
 plot_trajectory: True
 debug: False
-map_frame: map 
-joint_convergence_threshold: 0.001
+map_frame: map
+sample_period: 0.05
+fill_velocity_values: True
+joint_convergence_threshold: 0.01
 wiggle_precision_threshold: 5
+tree_tick_rate: 0.1
+nWSR: None # None results in a nWSR estimation thats fine most of the time
 max_traj_length: 30 # not implemented
-sample_period: 0.05
+collision_time_threshold: 15
 joint_vel_limit:
- default: 0.25
+ default: 1
 joint_acceleration_limit: # multiply your desired acc limit with sample period
  default: 100
 joint_weights:
@@ -35,9 +39,6 @@ collision_avoidance:
  r_upper_arm_link: *high
  l_forearm_link: *high
  r_forearm_link: *high
-fill_velocity_values: True
-nWSR: None # None results in a nWSR estimation thats fine most of the time
-root_link: odom_combined
-collision_time_threshold: 15
-tree_tick_rate: 0.1
-slerp: True
+self_collision_matrix:
+ ignore: []
+ add: []

src/giskardpy/constraints.py

@@ -347,7 +347,7 @@ def get_constraint(self):
 
 
 class CartesianOrientationSlerp(CartesianConstraint):
- def __init__(self, god_map, root_link, tip_link, goal, weight=HIGH_WEIGHT, gain=6, max_speed=1):
+ def __init__(self, god_map, root_link, tip_link, goal, weight=HIGH_WEIGHT, gain=10, max_speed=1):
  super(CartesianOrientationSlerp, self).__init__(god_map, root_link, tip_link, goal, weight, gain, max_speed)
 
  def get_constraint(self):
@@ -389,10 +389,9 @@ def get_constraint(self):
 
  soft_constraints = OrderedDict()
 
- # axis, angle = sw.diffable_axis_angle_from_matrix((current_rotation.T * goal_rotation))
  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((gain * angle)/ max_speed, 1)
 
  q1 = sw.quaternion_from_matrix(current_rotation)

src/giskardpy/pybullet_world.py

@@ -103,7 +103,7 @@ def __should_flip_contact_info(self, contact_info):
  contact_info3 = ContactInfo(
  *[x for x in p.getClosestPoints(hack_id,
  contact_info.body_unique_id_a, 0.001) if
- np.allclose(x[8], -0.005)][0])
+ abs(x[8]+0.005) < 1e-5][0])
  return not(contact_info3.body_unique_id_b == contact_info.body_unique_id_a and
  contact_info3.link_index_b == contact_info.link_index_a)
  except Exception as e:

src/giskardpy/qp_problem_builder.py

@@ -213,14 +213,18 @@ def check_for_nan(self, name, p_array):
  if len(p_filtered) > 0:
  logging.logwarn(u'{} has the following nans:'.format(name))
  self.print_pandas_array(p_filtered)
+ else:
+ logging.loginfo(u'no nans')
 
- def check_for_big_numbers(self, name, p_array, big=1e10):
+ def check_for_big_numbers(self, name, p_array, big=1e5):
  # FIXME fails if condition is true on first entry
  p_filtered = p_array.apply(lambda x: zip(x.index[abs(x) > big].tolist(), x[x > big]), 1)
  p_filtered = p_filtered[p_filtered.apply(lambda x: len(x)) > 0]
  if len(p_filtered) > 0:
  logging.logwarn(u'{} has the following big numbers:'.format(name))
  self.print_pandas_array(p_filtered)
+ else:
+ logging.loginfo(u'no big numbers')
 
  def print_pandas_array(self, array):
  import pandas as pd

src/giskardpy/symengine_wrappers.py

@@ -639,19 +639,12 @@ def axis_angle_from_quaternion(x, y, z, w):
  :return: 4x1 Matrix
  :rtype: Matrix
  """
- # TODO buggy, angle goes from 0 - 2*pi instead of -pi - +pi
  w2 = se.sqrt(1 - w ** 2)
  angle = (2 * se.acos(w))
- # TODO decide on axis for 0 angle
- w2 = Max(w2, 1e-20)
- x = diffable_if_eq_zero(x, x / w2, 1)
- y = diffable_if_greater_eq_zero(w2-1e-10, y / w2, 0)
- z = diffable_if_greater_eq_zero(w2-1e-10, z / w2, 0)
- l = norm([x,y,z])
- x /= l
- y /= l
- z /= l
- # angle = normalize_angle(angle)
+ m = if_eq_zero(w2, 1, w2) # avoid /0
+ x = if_eq_zero(w2, 0, x / m)
+ y = if_eq_zero(w2, 0, y / m)
+ z = if_eq_zero(w2, 1, z / m)
  return se.Matrix([x, y, z]), angle
 
 

test/utils_for_tests.py

@@ -71,14 +71,14 @@ def compare_axis_angle(angle1, axis1, angle2, axis2, decimal=3):
  np.testing.assert_array_almost_equal(axis1, axis2, decimal=decimal)
  np.testing.assert_almost_equal(shortest_angular_distance(angle1, angle2), 0, decimal=decimal)
  except AssertionError:
- np.testing.assert_array_almost_equal(axis1, -axis2, decimal=decimal)
- np.testing.assert_almost_equal(shortest_angular_distance(angle1, abs(angle2 - 2 * pi)), 0, decimal=decimal)
- # if np.almo(axis1, axis2).all():
- #  # assert np.isclose(shortest_angular_distance(angle1, angle2), 0), '{} != {}'.format(angle1, angle2)
- # elif np.isclose(axis1, -axis2).all():
- # # assert np.isclose(shortest_angular_distance(angle1, abs(angle2 - 2 * pi)), 0), '{} != {}'.format(angle1, angle2)
- # else:
- # assert False, u'{} != {}'.format(axis1, axis2)
+ try:
+  np.testing.assert_array_almost_equal(axis1, -axis2, decimal=decimal)
+  np.testing.assert_almost_equal(shortest_angular_distance(angle1, abs(angle2 - 2 * pi)), 0, decimal=decimal)
+ except AssertionError:
+  np.testing.assert_almost_equal(shortest_angular_distance(angle1, 0), 0, decimal=decimal)
+  np.testing.assert_almost_equal(shortest_angular_distance(0, angle2), 0, decimal=decimal)
+  assert not np.any(np.isnan(axis1))
+  assert not np.any(np.isnan(axis2))
 
 
 def compare_poses(pose1, pose2, decimal=2):
@@ -181,6 +181,8 @@ def unit_vector(length, elements=None):
  def normalize(v):
  v = [round(x, 4) for x in v]
  l = np.linalg.norm(v)
+ if l == 0:
+ return [0,0,0,1]
  return [x / l for x in v]
 
  return st.builds(normalize, vector)