airobot.utils.arm_util

airobot.utils.arm_util.reach_ee_goal(pos, ori, get_func, pos_tol=0.01, ori_tol=0.02)[source]

Check if end effector reached goal or not. Returns true if both position and orientation goals have been reached within specified tolerance.

Parameters:
  • pos (list np.ndarray) – goal position.
  • ori (list or np.ndarray) – goal orientation. It can be: quaternion ([qx, qy, qz, qw], shape: \([4]\)) rotation matrix (shape: \([3, 3]\)) euler angles ([roll, pitch, yaw], shape: \([3]\)).
  • get_func (function) – name of the function with which we can get the current end effector pose.
  • pos_tol (float) – tolerance of position error.
  • ori_tol (float) – tolerance of orientation error.
Returns:

bool – If goal pose is reached or not.
airobot.utils.arm_util.reach_jnt_goal(goal, get_func, joint_name=None, max_error=0.01)[source]

Check if the joint reached the goal or not. The goal can be a desired velocity(s) or a desired position(s).

Parameters:
  • goal (np.ndarray) – goal positions or velocities.
  • get_func (function) – name of the function with which we can get the current joint values.
  • joint_name (str) – if it’s none, all the actuated joints are compared. Otherwise, only the specified joint is compared.
  • max_error (float) – tolerance of error.
Returns:

bool – if the goal is reached or not.
airobot.utils.arm_util.wait_to_reach_ee_goal(pos, ori, get_func, get_func_derv=None, timeout=10.0, pos_tol=0.01, ori_tol=0.02)[source]

Block the code to wait for the end effector to reach its specified goal pose (must be below both position and orientation threshold).

Parameters:
  • pos (list) – goal position.
  • ori (list or np.ndarray) – goal orientation. It can be: quaternion ([qx, qy, qz, qw], shape: \([4]\)) rotation matrix (shape: \([3, 3]\)) euler angles ([roll, pitch, yaw], shape: \([3]\)).
  • get_func (function) – name of the function with which we can get the end effector pose.
  • get_func_derv (function) – the name of the function with which we can get end effector velocities.
  • timeout (float) – maximum waiting time.
  • pos_tol (float) – tolerance of position error.
  • ori_tol (float) – tolerance of orientation error.
Returns:

bool – If end effector reached goal or not.
airobot.utils.arm_util.wait_to_reach_jnt_goal(goal, get_func, joint_name=None, get_func_derv=None, timeout=10.0, max_error=0.01)[source]

Block the code to wait for the joint moving to the specified goal. The goal can be a desired velocity(s) or a desired position(s). if get_func returns the poisitions (velocities), then the get_func_derv should return the velocities (accelerations) (if provided). If the robot cannot reach the goal, providing get_func_derv can prevent the program from waiting until timeout. It uses the derivative information to make a faster judgement.

Parameters:
  • goal (float or list) – goal positions or velocities.
  • get_func (function) – name of the function with which we can get the current joint values.
  • joint_name (str) – if it’s none, all the actuated joints are compared. Otherwise, only the specified joint is compared.W
  • get_func_derv (function) – the name of the function with which we can get the derivative of the joint values.
  • timeout (float) – maximum waiting time.
  • max_error (float) – tolerance of error.
Returns:

bool – if the goal is reached or not.