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
