def set_pos(self, pos): """ Set the gripper position. Function internally maps values from API position range to URScript position range. After sending position command, update internal position variable by sending urscript program to controller. Args: pos (float): Desired gripper position. """ pos = clamp(pos, self.cfgs.EETOOL.OPEN_ANGLE, self.cfgs.EETOOL.CLOSE_ANGLE) if not self._gazebo_sim: urscript = self._get_new_urscript() pos = int(pos * self.cfgs.EETOOL.POSITION_SCALING) urscript.set_gripper_position(pos) urscript.sleep(0.1) gripper_cmd = urscript() else: gripper_cmd = GripperCommandActionGoal() gripper_cmd.goal.command.position = pos self._pub_command.publish(gripper_cmd) time.sleep(1.0) if not self._gazebo_sim: self._get_current_pos_urscript()
def set_noise(self, body_id, link_id, rgb1, rgb2, fraction=0.9): """ Apply noise to the texture. Args: body_id (int): body index. link_id (int): link index in the body. rgb1 (list or np.ndarray): background rgb color (shape: :math:`[3,]`). rgb2 (list or np.ndarray): color of random noise foreground color (shape: :math:`[3,]`). fraction (float): fraction of pixels with foreground color. """ if not self._check_link_has_tex(body_id, link_id): return rgb1 = np.array(rgb1).flatten() rgb2 = np.array(rgb2).flatten() tex_id, height, width = self.texture_dict[body_id][link_id] fraction = clamp(fraction, 0.0, 1.0) mask = np.random.uniform(size=(height, width)) < fraction new_color = np.tile(rgb1, (height, width, 1)) new_color[mask, :] = rgb2 p.changeTexture(tex_id, new_color.flatten(), width, height, physicsClientId=self._pb_id)
def set_pos(self, pos, wait=True): """ Set the gripper position. Args: pos (float): joint position. wait (bool): wait until the joint position is set to the target position. Returns: bool: A boolean variable representing if the action is successful at the moment when the function exits. """ joint_name = self.jnt_names[0] tgt_pos = arutil.clamp(pos, self.gripper_open_angle, self.gripper_close_angle) jnt_id = self.jnt_to_id[joint_name] self._pb.setJointMotorControl2(self.robot_id, jnt_id, self._pb.POSITION_CONTROL, targetPosition=tgt_pos, force=self._max_torque) if not self._pb.in_realtime_mode(): self._set_rest_joints(tgt_pos) success = False if self._pb.in_realtime_mode() and wait: success = wait_to_reach_jnt_goal( tgt_pos, get_func=self.get_pos, joint_name=joint_name, get_func_derv=self.get_vel, timeout=self.cfgs.ARM.TIMEOUT_LIMIT, max_error=self.cfgs.ARM.MAX_JOINT_ERROR) return success
def scale_motion(self, vel_scale=1.0, acc_scale=1.0): """ Sets the maximum velocity and acceleration for the robot motion. Specified as a fraction from 0.0 - 1.0 of the maximum velocity and acceleration specified in the MoveIt joint limits configuration file. Args: vel_scale (float): velocity scale, Defaults to 1.0. acc_scale (float): acceleration scale, Defaults to 1.0. """ vel_scale = arutil.clamp(vel_scale, 0.0, 1.0) acc_scale = arutil.clamp(acc_scale, 0.0, 1.0) self.moveit_group.set_max_velocity_scaling_factor(vel_scale) self.moveit_group.set_max_acceleration_scaling_factor(acc_scale) self._motion_vel = self.max_vel * vel_scale self._motion_acc = self.max_acc * acc_scale
def set_speed(self, speed): """ Set the default speed which the gripper should move at. Args: speed (int): Desired gripper speed (0 min, 255 max). """ speed = int(clamp(speed, 0, 255)) if not self._gazebo_sim: urscript = self._get_new_urscript() urscript.set_gripper_speed(speed) urscript.sleep(0.1) self._pub_command.publish(urscript())
def _scale_gripper_angle(self, command): """ Convert the command in [-1, 1] to the actual gripper angle. command = -1 means open the gripper. command = 1 means close the gripper. Args: command (float): a value between -1 and 1. -1 means open the gripper. 1 means close the gripper. Returns: float: the actual gripper angle corresponding to the command. """ command = clamp(command, -1.0, 1.0) close_ang = self.robot.arm.eetool.gripper_close_angle open_ang = self.robot.arm.eetool.gripper_open_angle cmd_ang = (command + 1) / 2.0 * (close_ang - open_ang) + open_ang return cmd_ang
def set_jpos(self, pos, wait=True, ignore_physics=False): """ Set the gripper position. Args: pos (float): joint position. wait (bool): wait until the joint position is set to the target position. Returns: bool: A boolean variable representing if the action is successful at the moment when the function exits. """ tgt_pos = arutil.clamp( pos, min(self.gripper_open_angle, self.gripper_close_angle), max(self.gripper_open_angle, self.gripper_close_angle)) tgt_pos = [tgt_pos] * len(self.gripper_jnt_ids) if ignore_physics: self._zero_vel_mode() self._hard_reset([pos]*len(self.gripper_jnt_ids)) success = True else: self._pb.setJointMotorControlArray(self.robot_id, self.gripper_jnt_ids, self._pb.POSITION_CONTROL, targetPositions=tgt_pos, forces=[self._max_torque] * len(self.gripper_jnt_ids)) success = False if self._pb.in_realtime_mode() and wait: success = wait_to_reach_jnt_goal( tgt_pos[0], get_func=self.get_jpos, joint_name=self.jnt_names[0], get_func_derv=self.get_jvel, timeout=self.cfgs.ARM.TIMEOUT_LIMIT, max_error=self.cfgs.ARM.MAX_JOINT_ERROR ) return success