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