def controller(self, obs: dict):
# Tolerances on reaching keyframes
posePosTol = 0.01
poseAxangTol = 0.09
# Which keyframe are we in?
if self.poseNum == 0:
rob0GoalPos = self.p_rob0_1
rob1GoalPos = self.p_rob1_1
rob0GoalAxAng = self.axang_rob0_1
rob1GoalAxAng = self.axang_rob1_1
elif self.poseNum == 1:
rob0GoalPos = self.p_rob0_2
rob1GoalPos = self.p_rob1_2
rob0GoalAxAng = self.axang_rob0_2
rob1GoalAxAng = self.axang_rob1_2
elif self.poseNum >= 2:
rob0GoalPos = self.p_rob0_3
rob1GoalPos = self.p_rob1_3
rob0GoalAxAng = self.axang_rob0_3
rob1GoalAxAng = self.axang_rob1_3
rob0OrientationError = T.get_orientation_error(
obs["robot0_eef_quat"], T.axisangle2quat(rob0GoalAxAng))
rob1OrientationError = T.get_orientation_error(
obs["robot1_eef_quat"], T.axisangle2quat(rob1GoalAxAng))
# Absolute pose control, so actions are just the poses we want to get to
posActionRob0 = self.p_rob0_1
posActionRob1 = self.p_rob1_1
axangActionRob0 = self.axang_rob0_1
axangActionRob1 = self.axang_rob1_1
# If we have reached the next keyframe, increment pose counter
if np.linalg.norm(
posActionRob0 / self.kpp) < posePosTol and np.linalg.norm(
rob0OrientationError) < poseAxangTol and np.linalg.norm(
posActionRob1 /
self.kpp) < posePosTol and np.linalg.norm(
rob1OrientationError) < poseAxangTol:
if self.poseNum <= 1:
self.poseNum = self.poseNum + 1
elif self.poseNum == 2:
print("done!")
self.poseNum = 3
return np.hstack((posActionRob0, axangActionRob0, posActionRob1,
axangActionRob1)).tolist()
def _clip_ik_input(self, dpos, rotation):
"""
Helper function that clips desired ik input deltas into a valid range.
Args:
dpos (np.array): a 3 dimensional array corresponding to the desired
change in x, y, and z end effector position.
rotation (np.array): relative rotation in scaled axis angle form (ax, ay, az)
corresponding to the (relative) desired orientation of the end effector.
Returns:
2-tuple:
- (np.array) clipped dpos
- (np.array) clipped rotation
"""
# scale input range to desired magnitude
if dpos.any():
dpos, _ = T.clip_translation(dpos, self.ik_pos_limit)
# Map input to quaternion
rotation = T.axisangle2quat(rotation)
# Clip orientation to desired magnitude
rotation, _ = T.clip_rotation(rotation, self.ik_ori_limit)
return dpos, rotation
def _randomize_direction(self, name):
"""
Helper function to randomize direction of a specific light source
Args:
name (str): Name of the lighting source to randomize for
"""
# sample a small, random axis-angle delta rotation
random_axis, random_angle = trans.random_axis_angle(angle_limit=self.direction_perturbation_size, random_state=self.random_state)
random_delta_rot = trans.quat2mat(trans.axisangle2quat(random_axis * random_angle))
# rotate direction by this delta rotation and set the new direction
new_dir = random_delta_rot.dot(self._defaults[name]['dir'])
self.set_dir(
name,
new_dir,
)
def _randomize_rotation(self, name):
"""
Helper function to randomize orientation of a specific camera
Args:
name (str): Name of the camera to randomize for
"""
# sample a small, random axis-angle delta rotation
random_axis, random_angle = trans.random_axis_angle(angle_limit=self.rotation_perturbation_size, random_state=self.random_state)
random_delta_rot = trans.quat2mat(trans.axisangle2quat(random_axis * random_angle))
# compute new rotation and set it
base_rot = trans.quat2mat(trans.convert_quat(self._defaults[name]['quat'], to='xyzw'))
new_rot = random_delta_rot.T.dot(base_rot)
new_quat = trans.convert_quat(trans.mat2quat(new_rot), to='wxyz')
self.set_quat(
name,
new_quat,
)
def set_goal(self, action, set_pos=None, set_ori=None):
"""
Sets goal based on input @action. If self.impedance_mode is not "fixed", then the input will be parsed into the
delta values to update the goal position / pose and the kp and/or damping_ratio values to be immediately updated
internally before executing the proceeding control loop.
Note that @action expected to be in the following format, based on impedance mode!
:Mode `'fixed'`: [joint pos command]
:Mode `'variable'`: [damping_ratio values, kp values, joint pos command]
:Mode `'variable_kp'`: [kp values, joint pos command]
Args:
action (Iterable): Desired relative joint position goal state
set_pos (Iterable): If set, overrides @action and sets the desired absolute eef position goal state
set_ori (Iterable): IF set, overrides @action and sets the desired absolute eef orientation goal state
"""
# Update state
self.update()
# Parse action based on the impedance mode, and update kp / kd as necessary
if self.impedance_mode == "variable":
damping_ratio, kp, delta = action[:6], action[6:12], action[12:]
self.kp = np.clip(kp, self.kp_min, self.kp_max)
self.kd = 2 * np.sqrt(self.kp) * np.clip(
damping_ratio, self.damping_ratio_min, self.damping_ratio_max)
elif self.impedance_mode == "variable_kp":
kp, delta = action[:6], action[6:]
self.kp = np.clip(kp, self.kp_min, self.kp_max)
self.kd = 2 * np.sqrt(self.kp) # critically damped
else: # This is case "fixed"
delta = action
# If we're using deltas, interpret actions as such
if self.use_delta:
if delta is not None:
scaled_delta = self.scale_action(delta)
if not self.use_ori:
# Set default control for ori since user isn't actively controlling ori
set_ori = np.array([[-1., 0., 0.], [0., 1., 0.],
[0., 0., -1.]])
else:
scaled_delta = []
# Else, interpret actions as absolute values
else:
set_pos = delta[:3]
# Set default control for ori if we're only using position control
set_ori = T.quat2mat(T.axisangle2quat(delta[3:6])) if self.use_ori else \
np.array([[-1., 0., 0.], [0., 1., 0.], [0., 0., -1.]])
scaled_delta = []
# We only want to update goal orientation if there is a valid delta ori value
# use math.isclose instead of numpy because numpy is slow
bools = [
0. if math.isclose(elem, 0.) else 1. for elem in scaled_delta[3:]
]
if sum(bools) > 0.:
self.goal_ori = set_goal_orientation(
scaled_delta[3:],
self.ee_ori_mat,
orientation_limit=self.orientation_limits,
set_ori=set_ori)
self.goal_pos = set_goal_position(scaled_delta[:3],
self.ee_pos,
position_limit=self.position_limits,
set_pos=set_pos)
if self.interpolator_pos is not None:
self.interpolator_pos.set_goal(self.goal_pos)
if self.interpolator_ori is not None:
self.ori_ref = np.array(
self.ee_ori_mat
) # reference is the current orientation at start
self.interpolator_ori.set_goal(
orientation_error(
self.goal_ori,
self.ori_ref)) # goal is the total orientation error
self.relative_ori = np.zeros(
3) # relative orientation always starts at 0
def _get_geom_attrs(self):
"""
Creates geom elements that will be passed to superclass CompositeObject constructor
Returns:
dict: args to be used by CompositeObject to generate geoms
"""
full_size = np.array((
self.body_half_size,
self.body_half_size + self.handle_length * 2,
self.body_half_size,
))
# Initialize dict of obj args that we'll pass to the CompositeObject constructor
base_args = {
"total_size": full_size / 2.0,
"name": self.name,
"locations_relative_to_center": True,
"obj_types": "all",
}
site_attrs = []
obj_args = {}
# Initialize geom lists
self._handle0_geoms = []
self._handle1_geoms = []
# Add main pot body
# Base geom
name = f"base"
self.pot_base = [name]
add_to_dict(
dic=obj_args,
geom_types="box",
geom_locations=(0, 0,
-self.body_half_size[2] + self.thickness / 2),
geom_quats=(1, 0, 0, 0),
geom_sizes=np.array([
self.body_half_size[0], self.body_half_size[1],
self.thickness / 2
]),
geom_names=name,
geom_rgbas=None if self.use_texture else self.rgba_body,
geom_materials="pot_mat" if self.use_texture else None,
geom_frictions=None,
density=self.density,
)
# Walls
x_off = np.array([
0, -(self.body_half_size[0] - self.thickness / 2), 0,
self.body_half_size[0] - self.thickness / 2
])
y_off = np.array([
-(self.body_half_size[1] - self.thickness / 2), 0,
self.body_half_size[1] - self.thickness / 2, 0
])
w_vals = np.array([
self.body_half_size[1], self.body_half_size[0],
self.body_half_size[1], self.body_half_size[0]
])
r_vals = np.array([np.pi / 2, 0, -np.pi / 2, np.pi])
for i, (x, y, w, r) in enumerate(zip(x_off, y_off, w_vals, r_vals)):
add_to_dict(
dic=obj_args,
geom_types="box",
geom_locations=(x, y, 0),
geom_quats=T.convert_quat(T.axisangle2quat(np.array([0, 0,
r])),
to="wxyz"),
geom_sizes=np.array(
[self.thickness / 2, w, self.body_half_size[2]]),
geom_names=f"body{i}",
geom_rgbas=None if self.use_texture else self.rgba_body,
geom_materials="pot_mat" if self.use_texture else None,
geom_frictions=None,
density=self.density,
)
# Add handles
main_bar_size = np.array([
self.handle_width / 2 + self.handle_radius,
self.handle_radius,
self.handle_radius,
])
side_bar_size = np.array(
[self.handle_radius, self.handle_length / 2, self.handle_radius])
handle_z = self.body_half_size[2] - self.handle_radius
for i, (g_list, handle_side, rgba) in enumerate(
zip([self._handle0_geoms, self._handle1_geoms], [1.0, -1.0],
[self.rgba_handle_0, self.rgba_handle_1])):
handle_center = np.array(
(0,
handle_side * (self.body_half_size[1] + self.handle_length),
handle_z))
# Solid handle case
if self.solid_handle:
name = f"handle{i}"
g_list.append(name)
add_to_dict(
dic=obj_args,
geom_types="box",
geom_locations=handle_center,
geom_quats=(1, 0, 0, 0),
geom_sizes=np.array([
self.handle_width / 2, self.handle_length / 2,
self.handle_radius
]),
geom_names=name,
geom_rgbas=None if self.use_texture else rgba,
geom_materials=f"handle{i}_mat"
if self.use_texture else None,
geom_frictions=(self.handle_friction, 0.005, 0.0001),
density=self.density,
)
# Hollow handle case
else:
# Center bar
name = f"handle{i}_c"
g_list.append(name)
add_to_dict(
dic=obj_args,
geom_types="box",
geom_locations=handle_center,
geom_quats=(1, 0, 0, 0),
geom_sizes=main_bar_size,
geom_names=name,
geom_rgbas=None if self.use_texture else rgba,
geom_materials=f"handle{i}_mat"
if self.use_texture else None,
geom_frictions=(self.handle_friction, 0.005, 0.0001),
density=self.density,
)
# Side bars
for bar_side, suffix in zip([-1.0, 1.0], ["-", "+"]):
name = f"handle{i}_{suffix}"
g_list.append(name)
add_to_dict(
dic=obj_args,
geom_types="box",
geom_locations=(
bar_side * self.handle_width / 2,
handle_side *
(self.body_half_size[1] + self.handle_length / 2),
handle_z,
),
geom_quats=(1, 0, 0, 0),
geom_sizes=side_bar_size,
geom_names=name,
geom_rgbas=None if self.use_texture else rgba,
geom_materials=f"handle{i}_mat"
if self.use_texture else None,
geom_frictions=(self.handle_friction, 0.005, 0.0001),
density=self.density,
)
# Add relevant site
handle_site = self.get_site_attrib_template()
handle_name = f"handle{i}"
handle_site.update({
"name":
handle_name,
"pos":
array_to_string(handle_center -
handle_side * np.array([0, 0.005, 0])),
"size":
"0.005",
"rgba":
rgba,
})
site_attrs.append(handle_site)
# Add to important sites
self._important_sites[
f"handle{i}"] = self.naming_prefix + handle_name
# Add pot body site
pot_site = self.get_site_attrib_template()
center_name = "center"
pot_site.update({
"name": center_name,
"size": "0.005",
})
site_attrs.append(pot_site)
# Add to important sites
self._important_sites["center"] = self.naming_prefix + center_name
# Add back in base args and site args
obj_args.update(base_args)
obj_args["sites"] = site_attrs # All sites are part of main (top) body
# Return this dict
return obj_args
def set_goal_orientation(delta,
current_orientation,
orientation_limit=None,
set_ori=None):
"""
Calculates and returns the desired goal orientation, clipping the result accordingly to @orientation_limits.
@delta and @current_orientation must be specified if a relative goal is requested, else @set_ori must be
an orientation matrix specified to define a global orientation
Args:
delta (np.array): Desired relative change in orientation, in axis-angle form [ax, ay, az]
current_orientation (np.array): Current orientation, in rotation matrix form
orientation_limit (None or np.array): 2d array defining the (min, max) limits of permissible orientation goal commands
set_ori (None or np.array): If set, will ignore @delta and set the goal orientation to this value
Returns:
np.array: calculated goal orientation in absolute coordinates
Raises:
ValueError: [Invalid orientation_limit shape]
"""
# directly set orientation
if set_ori is not None:
goal_orientation = set_ori
# otherwise use delta to set goal orientation
else:
# convert axis-angle value to rotation matrix
quat_error = trans.axisangle2quat(delta)
rotation_mat_error = trans.quat2mat(quat_error)
goal_orientation = np.dot(rotation_mat_error, current_orientation)
# check for orientation limits
if np.array(orientation_limit).any():
if orientation_limit.shape != (2, 3):
raise ValueError("Orientation limit should be shaped (2,3) "
"but is instead: {}".format(
orientation_limit.shape))
# Convert to euler angles for clipping
euler = trans.mat2euler(goal_orientation)
# Clip euler angles according to specified limits
limited = False
for idx in range(3):
if orientation_limit[0][idx] < orientation_limit[1][
idx]: # Normal angle sector meaning
if orientation_limit[0][idx] < euler[idx] < orientation_limit[
1][idx]:
continue
else:
limited = True
dist_to_lower = euler[idx] - orientation_limit[0][idx]
if dist_to_lower > np.pi:
dist_to_lower -= 2 * np.pi
elif dist_to_lower < -np.pi:
dist_to_lower += 2 * np.pi
dist_to_higher = euler[idx] - orientation_limit[1][idx]
if dist_to_lower > np.pi:
dist_to_higher -= 2 * np.pi
elif dist_to_lower < -np.pi:
dist_to_higher += 2 * np.pi
if dist_to_lower < dist_to_higher:
euler[idx] = orientation_limit[0][idx]
else:
euler[idx] = orientation_limit[1][idx]
else: # Inverted angle sector meaning
if orientation_limit[0][idx] < euler[idx] or euler[
idx] < orientation_limit[1][idx]:
continue
else:
limited = True
dist_to_lower = euler[idx] - orientation_limit[0][idx]
if dist_to_lower > np.pi:
dist_to_lower -= 2 * np.pi
elif dist_to_lower < -np.pi:
dist_to_lower += 2 * np.pi
dist_to_higher = euler[idx] - orientation_limit[1][idx]
if dist_to_lower > np.pi:
dist_to_higher -= 2 * np.pi
elif dist_to_lower < -np.pi:
dist_to_higher += 2 * np.pi
if dist_to_lower < dist_to_higher:
euler[idx] = orientation_limit[0][idx]
else:
euler[idx] = orientation_limit[1][idx]
if limited:
goal_orientation = trans.euler2mat(
np.array([euler[0], euler[1], euler[2]]))
return goal_orientation
