def set_jpos(self,
position,
arm=None,
joint_name=None,
wait=True,
*args,
**kwargs):
"""
Move the arm to the specified joint position(s).
Args:
position (float or list): desired joint position(s).
arm (str): If not provided, position should be a list and all
actuated joints will be moved. If provided, only half the
joints will move, corresponding to which arm was specified.
Value should match arm names in cfg file.
joint_name (str): If not provided, position should be a list
and all the actuated joints will be moved to the specified
positions. If provided, only the specified joint will
be moved to the desired joint position. If joint_name is
provided, then arm argument should also be provided, and
specified joint name must correspond to joint names for the
specified arm.
wait (bool): whether to block the code and wait
for the action to complete.
Returns:
bool: A boolean variable representing if the action is successful
at the moment when the function exits.
"""
position = copy.deepcopy(position)
success = False
if arm is None:
if len(position) != self.dual_arm_dof:
raise ValueError('Position should contain %d '
'elements if arm is not provided' %
self.dual_arm_dof)
tgt_pos = position
self._pb.setJointMotorControlArray(self.robot_id,
self.arm_jnt_ids,
self._pb.POSITION_CONTROL,
targetPositions=tgt_pos,
forces=self._max_torques)
if self._pb.in_realtime_mode() and wait:
success = wait_to_reach_jnt_goal(
tgt_pos,
get_func=self.get_jpos,
joint_name=joint_name,
get_func_derv=self.get_jvel,
timeout=self.cfgs.ARM.TIMEOUT_LIMIT,
max_error=self.cfgs.ARM.MAX_JOINT_ERROR)
else:
if arm not in self.arms:
raise ValueError('Valid arm name must be specified '
'("%s" or "%s")' %
(self._arm_names[0], self._arm_names[1]))
success = self.arms[arm].set_jpos(position,
joint_name=joint_name,
wait=wait)
return success
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 set_jvel(self, velocity, joint_name=None, wait=False, *args, **kwargs):
"""
Move the arm with the specified joint velocity(ies). Applies regulation
position command to the compliant joints after sending driven commands.
Args:
velocity (float or list): desired joint velocity(ies).
joint_name (str): If not provided, velocity should be a list
and all the actuated joints will be moved in the specified
velocities. If provided, only the specified joint will
be moved in the desired joint velocity.
wait (bool): whether to block the code and wait
for the action to complete.
Returns:
bool: A boolean variable representing if the action is successful
at the moment when the function exits.
"""
velocity = copy.deepcopy(velocity)
success = False
if joint_name is None:
velocity = copy.deepcopy(velocity)
if len(velocity) != self.arm_dof:
raise ValueError('Velocity should contain %d elements '
'if the joint_name is not '
'provided' % self.arm_dof)
tgt_vel = velocity
self._pb.setJointMotorControlArray(self.robot_id,
self.arm_jnt_ids,
self._pb.VELOCITY_CONTROL,
targetVelocities=tgt_vel,
forces=self._max_torques)
else:
if joint_name not in self.arm_jnt_names:
raise TypeError('Joint name [%s] is not in the arm'
' joint list!' % joint_name)
else:
tgt_vel = velocity
arm_jnt_idx = self.arm_jnt_names.index(joint_name)
max_torque = self._max_torques[arm_jnt_idx]
jnt_id = self.jnt_to_id[joint_name]
self._pb.setJointMotorControl2(self.robot_id,
jnt_id,
self._pb.VELOCITY_CONTROL,
targetVelocity=tgt_vel,
force=max_torque)
self.set_compliant_jpos()
if self._pb.in_realtime_mode():
if wait:
success = wait_to_reach_jnt_goal(
tgt_vel,
get_func=self.get_jvel,
joint_name=joint_name,
timeout=self.cfgs.ARM.TIMEOUT_LIMIT,
max_error=self.cfgs.ARM.MAX_JOINT_VEL_ERROR)
else:
success = True
return success
def set_jpos(self, position, joint_name=None, wait=True, *args, **kwargs):
"""
Move the arm to the specified joint position(s). Applies regulation
position command to the compliant joints after sending driven
joint commands.
Args:
position (float or list): desired joint position(s).
joint_name (str): If not provided, position should be a list
and all the actuated joints will be moved to the specified
positions. If provided, only the specified joint will
be moved to the desired joint position.
wait (bool): whether to block the code and wait
for the action to complete.
Returns:
bool: A boolean variable representing if the action is successful
at the moment when the function exits.
"""
position = copy.deepcopy(position)
success = False
if joint_name is None:
if len(position) != self.arm_dof:
raise ValueError('Position should contain %d'
'elements if the joint_name'
' is not provided' % self.arm_dof)
tgt_pos = position
self._pb.setJointMotorControlArray(self.robot_id,
self.arm_jnt_ids,
self._pb.POSITION_CONTROL,
targetPositions=tgt_pos,
forces=self._max_torques)
else:
if joint_name not in self.arm_jnt_names:
raise TypeError('Joint name [%s] is not in the arm'
' joint list!' % joint_name)
else:
tgt_pos = position
arm_jnt_idx = self.arm_jnt_names.index(joint_name)
max_torque = self._max_torques[arm_jnt_idx]
jnt_id = self.jnt_to_id[joint_name]
self._pb.setJointMotorControl2(self.robot_id,
jnt_id,
self._pb.POSITION_CONTROL,
targetPosition=tgt_pos,
force=max_torque)
self.set_compliant_jpos()
if not self._step_sim_mode and wait:
success = wait_to_reach_jnt_goal(
tgt_pos,
get_func=self.get_jpos,
joint_name=joint_name,
get_func_derv=self.get_jvel,
timeout=self.cfgs.ARM.TIMEOUT_LIMIT,
max_error=self.cfgs.ARM.MAX_JOINT_ERROR)
return success
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
def set_jvel(self, velocity, joint_name=None, wait=False, *args, **kwargs):
"""
Set joint velocity command to the robot (units in rad/s).
Args:
velocity (float or list or flattened np.ndarray): list of target
joint velocity value(s)
(shape: :math:`[6,]` if list, otherwise a single value)
joint_name (str, optional): If not provided, velocity should be
list and all joints will be turned on at specified velocity.
Defaults to None.
wait (bool, optional): If True, block until robot reaches
desired joint velocity value(s). Defaults to False.
Returns:
bool: True if command was completed successfully, returns
False if wait flag is set to False.
"""
velocity = copy.deepcopy(velocity)
success = False
# TODO improve velocity control performance
ar.log_warn('Velocity control is not well tested!!!')
if self._gazebo_sim:
raise NotImplementedError('cannot set_jvel in Gazebo')
if joint_name is None:
if len(velocity) != self.arm_dof:
raise ValueError('Velocity should contain %d elements'
'if the joint name is not '
'provided' % self.arm_dof)
tgt_vel = velocity
else:
if not isinstance(velocity, float):
raise TypeError('velocity should be individual float value'
'if joint_name is provided')
if joint_name not in self.arm_jnt_names_set:
raise TypeError('Joint name [%s] is not in the arm'
' joint list!' % joint_name)
else:
tgt_vel = [0.0] * len(self.arm_jnt_names)
arm_jnt_idx = self.arm_jnt_names.index(joint_name)
tgt_vel[arm_jnt_idx] = velocity
if self._use_urscript:
prog = 'speedj([%f, %f, %f, %f, %f, %f],' \
' a=%f)' % (tgt_vel[0],
tgt_vel[1],
tgt_vel[2],
tgt_vel[3],
tgt_vel[4],
tgt_vel[5],
self._motion_acc)
self._send_urscript(prog)
else:
# self._pub_joint_vel(tgt_vel)
raise NotImplementedError('set_jvel only works '
'with use_urscript=True')
if wait:
success = wait_to_reach_jnt_goal(
velocity,
get_func=self.get_jvel,
joint_name=joint_name,
timeout=self.cfgs.ARM.TIMEOUT_LIMIT,
max_error=self.cfgs.ARM.MAX_JOINT_VEL_ERROR)
return success
def set_jpos(self, position, joint_name=None, wait=True, *args, **kwargs):
"""
Method to send a joint position command to the robot (units in rad).
Args:
position (float or list or flattened np.ndarray):
desired joint position(s)
(shape: :math:`[6,]` if list, otherwise a single value).
joint_name (str): If not provided, position should be a list and
all actuated joints will be moved to specified positions. If
provided, only specified joint will move. Defaults to None.
wait (bool): whether position command should be blocking or non
blocking. Defaults to True.
Returns:
bool: True if command was completed successfully, returns
False if wait flag is set to False.
"""
position = copy.deepcopy(position)
success = False
if joint_name is None:
if len(position) != self.arm_dof:
raise ValueError('position should contain %d elements if '
'joint_name is not provided' % self.arm_dof)
tgt_pos = position
else:
if not isinstance(position, numbers.Number):
raise TypeError('position should be individual float value'
' if joint_name is provided')
if joint_name not in self.arm_jnt_names_set:
raise TypeError('Joint name [%s] is not in the arm'
' joint list!' % joint_name)
else:
tgt_pos = self.get_jpos()
arm_jnt_idx = self.arm_jnt_names.index(joint_name)
tgt_pos[arm_jnt_idx] = position
if self._use_urscript:
prog = 'movej([%f, %f, %f, %f, %f, %f],' \
' a=%f, v=%f)' % (tgt_pos[0],
tgt_pos[1],
tgt_pos[2],
tgt_pos[3],
tgt_pos[4],
tgt_pos[5],
self._motion_acc,
self._motion_vel)
self._send_urscript(prog)
if wait:
success = wait_to_reach_jnt_goal(
position,
get_func=self.get_jpos,
joint_name=joint_name,
get_func_derv=self.get_jvel,
timeout=self.cfgs.ARM.TIMEOUT_LIMIT,
max_error=self.cfgs.ARM.MAX_JOINT_ERROR)
else:
self.moveit_group.set_joint_value_target(tgt_pos)
success = self.moveit_group.go(tgt_pos, wait=wait)
return success
def set_jpos(self,
position,
joint_name=None,
wait=True,
ignore_physics=False,
*args,
**kwargs):
"""
Move the arm to the specified joint position(s).
Args:
position (float or list): desired joint position(s).
joint_name (str): If not provided, position should be a list
and all the actuated joints will be moved to the specified
positions. If provided, only the specified joint will
be moved to the desired joint position.
wait (bool): whether to block the code and wait
for the action to complete.
ignore_physics (bool): hard reset the joints to the target joint
positions. It's best only to do this at the start,
while not running the simulation. It will overrides
all physics simulation.
Returns:
bool: A boolean variable representing if the action is successful
at the moment when the function exits.
"""
position = copy.deepcopy(position)
success = False
if joint_name is None:
if len(position) != self.arm_dof:
raise ValueError('Position should contain %d '
'elements if the joint_name'
' is not provided' % self.arm_dof)
tgt_pos = position
if ignore_physics:
# we need to set the joints to velocity control mode
# so that the reset takes effect. Otherwise, the joints
# will just go back to the original positions
self.set_jvel([0.] * self.arm_dof)
for idx, jnt in enumerate(self.arm_jnt_names):
self.reset_joint_state(jnt, tgt_pos[idx])
success = True
else:
self._pb.setJointMotorControlArray(self.robot_id,
self.arm_jnt_ids,
self._pb.POSITION_CONTROL,
targetPositions=tgt_pos,
forces=self._max_torques)
else:
if joint_name not in self.arm_jnt_names:
raise TypeError('Joint name [%s] is not in the arm'
' joint list!' % joint_name)
else:
tgt_pos = position
arm_jnt_idx = self.arm_jnt_names.index(joint_name)
max_torque = self._max_torques[arm_jnt_idx]
jnt_id = self.jnt_to_id[joint_name]
if ignore_physics:
self.set_jvel(0., joint_name)
self.reset_joint_state(joint_name, tgt_pos)
success = True
else:
self._pb.setJointMotorControl2(self.robot_id,
jnt_id,
self._pb.POSITION_CONTROL,
targetPosition=tgt_pos,
force=max_torque)
if self._pb.in_realtime_mode() and wait and not ignore_physics:
success = wait_to_reach_jnt_goal(
tgt_pos,
get_func=self.get_jpos,
joint_name=joint_name,
get_func_derv=self.get_jvel,
timeout=self.cfgs.ARM.TIMEOUT_LIMIT,
max_error=self.cfgs.ARM.MAX_JOINT_ERROR)
return success
