def test1():
print("Test 1")
# Repeat the first experiment in A. Ude et al. "Orientation in Cartesian
# space dynamic movement primitives"
tau = 3.5
dmp = dmpy.QuaternionDMP(25, alpha=48.0, beta=12.0, cs_alpha=2.0)
dmp.q0 = np.normalized(np.quaternion(0.3717, -0.4993, -0.6162, 0.4825))
dmp.go = np.normalized(np.quaternion(0.2471, 0.1797, 0.3182, -0.8974))
ts = np.linspace(0, 2, 1000)
q, omega, domega = dmp.rollout(ts, tau)
fig, (ax1, ax2, ax3) = plt.subplots(3, sharex=True)
ax1.plot(ts, quaternion.as_float_array(q))
ax1.set_xlim([0, 1.4])
ax1.legend([r'$q_{}$'.format(i) for i in 'wxyz'], loc='upper right')
ax2.plot(ts, omega)
ax2.legend([r'$\omega_{}$'.format(i) for i in 'xyz'], loc='upper right')
ax3.plot(ts, domega)
ax3.legend([r'$\dot\omega_{}$'.format(i) for i in 'xyz'],
loc='upper right')
plt.suptitle('DMP trajectory')
plt.show()
def _est_max_grad_dir(self, goal_pos: np.array) -> np.array:
current_state = self._sim.get_agent_state()
current_pos = current_state.position
if self.mode == "geodesic_path":
points = self._sim.get_straight_shortest_path_points(
self._sim.get_agent_state().position, goal_pos)
# Add a little offset as things get weird if
# points[1] - points[0] is anti-parallel with forward
if len(points) < 2:
return None
max_grad_dir = quaternion_from_two_vectors(
self._sim.forward_vector,
points[1] - points[0] + EPSILON *
np.cross(self._sim.up_vector, self._sim.forward_vector),
)
max_grad_dir.x = 0
max_grad_dir = np.normalized(max_grad_dir)
else:
current_rotation = self._sim.get_agent_state().rotation
current_dist = self._geo_dist(goal_pos)
best_geodesic_delta = -2 * self._max_delta
best_rotation = current_rotation
for _ in range(0, 360, self._sim.config.TURN_ANGLE):
sim_action = SimulatorActions.FORWARD.value
self._sim.step(sim_action)
new_delta = current_dist - self._geo_dist(goal_pos)
if new_delta > best_geodesic_delta:
best_rotation = self._sim.get_agent_state().rotation
best_geodesic_delta = new_delta
# If the best delta is within (1 - cos(TURN_ANGLE))% of the
# best delta (the step size), then we almost certainly have
# found the max grad dir and should just exit
if np.isclose(
best_geodesic_delta,
self._max_delta,
rtol=1 -
np.cos(np.deg2rad(self._sim.config.TURN_ANGLE)),
):
break
self._sim.set_agent_state(
current_pos,
self._sim.get_agent_state().rotation,
reset_sensors=False,
)
sim_action = SimulatorActions.LEFT.value
self._sim.step(sim_action)
self._reset_agent_state(current_state)
max_grad_dir = best_rotation
return max_grad_dir
def get_optimal_action(self, start, pose_estimate):
EPSILON = 1e-6
config_env = super().habitat_env._config
goal_pos = super().habitat_env.current_episode.goals[0].position
goal_radius = config_env.TASK.SUCCESS_DISTANCE
current_state = super().habitat_env.sim.get_agent_state()
position_estimate, rotation_estimate = self.ans_frame_to_hab(
start, pose_estimate)
if (super().habitat_env.sim.geodesic_distance(
position_estimate, goal_pos) <= goal_radius):
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: STOP".format(goal_pos, current_state.position))
return HabitatSimActions.STOP
points = super().habitat_env.sim.get_straight_shortest_path_points(
position_estimate, goal_pos)
# Add a little offset as things get weird if
# points[1] - points[0] is anti-parallel with forward
if len(points) < 2:
max_grad_dir = None
else:
max_grad_dir = quaternion_from_two_vectors(
super().habitat_env.sim.forward_vector,
points[1] - points[0] +
EPSILON * np.cross(super().habitat_env.sim.up_vector,
super().habitat_env.sim.forward_vector),
)
max_grad_dir.x = 0
max_grad_dir = np.normalized(max_grad_dir)
if max_grad_dir is None:
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: MOVE FORWARD".format(goal_pos, current_state.position))
return HabitatSimActions.MOVE_FORWARD
else:
alpha = angle_between_quaternions(max_grad_dir, rotation_estimate)
if alpha <= np.deg2rad(config_env.SIMULATOR.TURN_ANGLE) + EPSILON:
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: MOVE FORWARD".format(goal_pos, current_state.position))
return HabitatSimActions.MOVE_FORWARD
else:
if (angle_between_quaternions(max_grad_dir, rotation_estimate)
< alpha):
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: TURN LEFT".format(goal_pos, current_state.position))
return HabitatSimActions.TURN_LEFT
else:
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: TURN LEFT".format(goal_pos, current_state.position))
return HabitatSimActions.TURN_RIGHT
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: STOP".format(goal_pos, current_state.position))
return HabitatSimActions.STOP
def as_rotation_vector(q):
"""Convert input quaternion to the axis-angle representation
Note that if any of the input quaternions has norm zero, no error is
raised, but NaNs will appear in the output.
Parameters
----------
q: quaternion or array of quaternions
The quaternion(s) need not be normalized, but must all be nonzero
Returns
-------
rot: float array
Output shape is q.shape+(3,). Each vector represents the axis of
the rotation, with norm proportional to the angle of the rotation.
"""
return as_float_array(2 * np.log(np.normalized(q)))[..., 1:]
def as_rotation_vector(q):
"""Convert input quaternion to the axis-angle representation
Note that if any of the input quaternions has norm zero, no error is
raised, but NaNs will appear in the output.
Parameters
----------
q: quaternion or array of quaternions
The quaternion(s) need not be normalized, but must all be nonzero
Returns
-------
rot: float array
Output shape is q.shape+(3,). Each vector represents the axis of
the rotation, with norm proportional to the angle of the rotation.
"""
return as_float_array(2*np.log(np.normalized(q)))[..., 1:]
def __init__(self, q_r, q_d, normalize=False):
if not (q_r.dtype == np.quaternion) or not (q_d.dtype == np.quaternion):
raise ValueError("q_r and q_d must be of type np.quaternion. Instead received: {} and {}".format(
type(q_r), type(q_d)))
if (isinstance(q_r,np.ndarray) and isinstance(q_d,np.ndarray)) and (len(q_r)!=len(q_d)):
raise ValueError(
"q_r and q_d must be the same length. Instead received: {} and {}".format(len(q_r), len(q_d)))
if normalize:
self.q_d = q_d / np.norm(q_r)
self.q_r = np.normalized(q_r)
else:
self.q_r = q_r
self.q_d = q_d
if isinstance(self.q_r, np.ndarray):
self._len = len(self.q_r)
else:
self._len = 1
self.shape = tuple([len(self)])
def from_pose(cls,*args,**kwargs):
normalize = kwargs.get("normalize","True")
if (len(args)==1) and (len(kwargs) in [0,1]) and isinstance(args[0],np.ndarray):
return cls.from_quat_pose_array(args[0],normalize=normalize) # the original way
elif (len(args)==2) and (len(kwargs) in [0,1]):
a0, a1 = args[0], args[1]
if not isinstance(a0,np.ndarray):
a0 = np.array([a0])
if not isinstance(a1,np.ndarray):
a1 = np.array([a1])
if a0.dtype==np.quaternion: # assume a0 is rotation quaternion, a1 is translation vector
q_r = a0
translation = a1
elif a1.dtype==np.quaternion:
q_r = a1
translation = a0
else:
raise ValueError("At least one argument should be a rotation quaternion")
elif (len(args)==0) and (len(kwargs)==2):
if "rotq" in kwargs:
q_r = kwargs.get("rotq")
elif "q_r" in kwargs:
q_r = kwargs.get("q_r")
else:
raise ValueError("Keyword arguments must inclue a rotation quaternion")
if "translation" in kwargs:
translation = kwargs.get("translation")
elif "position" in kwargs:
translation = kwargs.get("translation")
else:
raise ValueError("Keyword arguments must inclue a translation vector")
else:
raise ValueError("Invalid input: use DualQuaternion.from_pose(q_r, translation)")
q_r = np.normalized(q_r)
q_d_float = np.zeros((len(q_r),4))
q_d_float[:,1:] = translation
q_d = 0.5 * quaternion.from_float_array(q_d_float) * q_r
return cls(q_r, q_d,normalize=normalize)
def nlerp(q1, q2, t):
q = q1 * (1 - t) + q2 * t
return np.normalized(q)
def get_optimal_gt_action(self, debug=False):
EPSILON = 1e-6
config_env = super().habitat_env._config
goal_pos = super().habitat_env.current_episode.goals[0].position
goal_radius = config_env.TASK.SUCCESS_DISTANCE
current_state = super().habitat_env.sim.get_agent_state()
# return HabitatSimActions.STOP
log_bool = self.total_num_steps % self.args.log_interval == 0
self.total_num_steps += 1
# add date
getdate = lambda: str(datetime.now()).split('.')[0]
# set true for debuging
if debug:
log = lambda w, x, y, z: print(
"\n{}\nGoal Position:{}\nCurrent Position:{}\nOptimal Action: {}\nGeodistic distance to goal: {}"
.format(getdate(), w, x, y, z))
else:
log = lambda w, x, y, z: print("\n{}\nGeodistic Dist: {}".format(
getdate(), z))
geoDist = super().habitat_env.sim.geodesic_distance(
current_state.position, goal_pos)
if (geoDist <= goal_radius):
if log_bool: log(goal_pos, current_state.position, 'STOP', geoDist)
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: STOP\nGeodistic Dist: {2}".format(goal_pos, current_state.position, geoDist))
return HabitatSimActions.STOP
points = super().habitat_env.sim.get_straight_shortest_path_points(
current_state.position, goal_pos)
# Add a little offset as things get weird if
# points[1] - points[0] is anti-parallel with forward
if len(points) < 2:
max_grad_dir = None
else:
max_grad_dir = quaternion_from_two_vectors(
super().habitat_env.sim.forward_vector,
points[1] - points[0] +
EPSILON * np.cross(super().habitat_env.sim.up_vector,
super().habitat_env.sim.forward_vector),
)
max_grad_dir.x = 0
max_grad_dir = np.normalized(max_grad_dir)
if max_grad_dir is None:
if log_bool:
log(goal_pos, current_state.position, 'FORWARD', geoDist)
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: MOVE FORWARD".format(goal_pos, current_state.position))
return HabitatSimActions.MOVE_FORWARD
else:
alpha = angle_between_quaternions(max_grad_dir,
current_state.rotation)
if alpha <= np.deg2rad(config_env.SIMULATOR.TURN_ANGLE) + EPSILON:
if log_bool:
log(goal_pos, current_state.position, 'FORWARD', geoDist)
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: MOVE FORWARD\nGeodistic Dist: {2}".format(goal_pos, current_state.position, geoDist))
return HabitatSimActions.MOVE_FORWARD
else:
if (angle_between_quaternions(max_grad_dir,
current_state.rotation) < alpha):
if log_bool:
log(goal_pos, current_state.position, 'LEFT', geoDist)
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: TURN LEFT\nGeodistic Dist: {2}".format(goal_pos, current_state.position, geoDist))
return HabitatSimActions.TURN_LEFT
else:
if log_bool:
log(goal_pos, current_state.position, 'RIGHT', geoDist)
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: TURN LEFT\nGeodistic Dist: {2}".format(goal_pos, current_state.position, geoDist))
return HabitatSimActions.TURN_RIGHT
if log_bool: log(goal_pos, current_state.position, 'STOP', geoDist)
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: STOP\nGeodistic Dist: {2}".format(goal_pos, current_state.position, geoDist))
return HabitatSimActions.STOP
def normalize(self):
q = np.normalized(self)
self.x = q.x
self.y = q.y
self.z = q.z
self.w = q.w
def normalized(self):
return np.normalized(self)
