panda_autograsp.functions package

Submodules

panda_autograsp.functions.conversions module

A number of usefull ros data_type conversion functions.

panda_autograsp.functions.conversions.transform_stamped_2_matrix(transform_stamped)[source]

Generate 4x4 homogeneous transformation matrix from a transform_stamped msg.

Parameters:transform_stamped (geometry_msgs.msgs.TransformStamped) – Stamped transform.
Returns:Homogeneous 4x4 transformation matrix.
Return type:numpy.array
panda_autograsp.functions.conversions.pose_msg_stamped_2_matrix(pose_msg)[source]

Generate 4x4 homogeneous transformation matrix from a pose_stamped msg.

Parameters:pose_msg (geometry_msgs.msgs.PoseStamped) – Stamped pose message.
Returns:Homogeneous 4x4 transformation matrix.
Return type:numpy.array

panda_autograsp.functions.functions module

This modules contains some additional functions which are not part of a specific module.

panda_autograsp.functions.functions.download_model(model, model_output='/home/ricks/panda_autograsp_ws/src/panda_autograsp/src/models', download_script_path='/home/ricks/panda_autograsp_ws/src/panda_autograsp/gqcnn/scripts/downloads/models/download_models.sh')[source]

This function downloads the Pretrained CNN models that are used in the panda_autograsp package, when they are not yet present on the system.

Parameters:
  • model (str) – Name of the model you want to download.
  • model_output (str, optional) – Path to the folder in which you want to place the downloaded models, by default DEFAULT_MODELS_PATH
  • download_script_path (str, optional) – Path to the download description script, by default GQCNN_DOWNLOAD_SCRIPT_PATH
Returns:

Error code Description
0 Model download successfull.
1 Model name invalid.
2 Model download error.
3 Model unzip error.
4 Something else went wrong.

Return type:

int

panda_autograsp.functions.functions.list_files(path='.', exclude=[], recursive=True, prepent_parent=False)[source]

Returns a list of files that are present in a folder.

Parameters:
  • path (str, optional) – Parent folder of which you want to list the files, by default .
  • exclude (list, optional) – A list of files you want to exclude, by default []
  • recursive (bool, optional) – Option specifying whether you also want to list files of subfolders, by default True
  • level (int, optional) – If recursive is enabled this specifies up till how many levels deep you want to list the files, by default 0 (Defined as all levels).
  • perpent_parent (bool, optional) – Options specifying whether you want to prepent the parent dir to the output paths.
Returns:

A list containing the relative paths of all the files in the parent folder.

Return type:

List

panda_autograsp.functions.functions.yes_or_no(question, add_options=True)[source]

Simple yes or no prompt.

Parameters:
  • question (str) – Question of the yes or no prompt.
  • add_options (bool) – Include option statement (Y/n) after the question.
Returns:

Boolean specifying if the user gave the right response.

Return type:

bool

panda_autograsp.functions.functions.draw_axis(img, corners, imgpts)[source]

Takes the corners in the chessboard (obtained using cv2.findChessboardCorners()) and axis points to draw a 3D axis.

Parameters:
  • img (numpy.ndarray) – Image
  • corners (pyton2.tuple) – Chess board corner points.
  • imgpts (nmpy.ndarray) – Axis points.
Returns:

Image on which the chessboard corners have been drawn.

Return type:

numpy.ndarray

panda_autograsp.functions.moveit module

Additional moveit helper functions.

panda_autograsp.functions.moveit.get_trajectory_duration(trajectory)[source]

This function returns the duration of a given moveit trajectory.

Parameters:trajectory (moveit_msgs.msg.DisplayTrajectory) – The computed display trajectory.
Returns:Trajectory duration in seconds.
Return type:float
panda_autograsp.functions.moveit.plan_exists(plan)[source]

This function can be used to check if a plan trajectory was computed.

Parameters:plan (moveit_msgs.msg.RobotTrajectory) – The computed robot trajectory.
Returns:Bool specifying if a trajectory is present
Return type:bool
panda_autograsp.functions.moveit.at_joint_target(current, desired, goal_tolerance)[source]

This function can be used to check if the move_group is already at the desired joint target.

Parameters:
  • current (list) – The current joint configuration.
  • desired (list) – The desired joint target.
  • goal_tolerance (float) – The planner target goal tolerance.
Returns:

Bool specifying if move_group is already at the goal target.

Return type:

bool

panda_autograsp.functions.moveit.add_collision_objects(scene_commander, collision_cfg)[source]

Add the collision objects that are specified in the collision_cfg collections.OrderedDict to the moveit planner scene. Supported constraint types are:

  • Box
  • Cube
  • Cylinder
  • Mesh

For more information see http://bit.ly/32GuuMN.

Parameters:
  • scene_commander (moveit_commander.PlanningSceneInterface) – The moveit scene commander object.
  • collision_cfg (collections.OrderedDict) – The dictionary specifying all the moveit scene constraints.
panda_autograsp.functions.moveit.wait_for_state_update(scene_commander, collision_obj, is_known=False, is_attached=False, timeout=4)[source]

Validate if collision object state change was successful.

Parameters:
  • scene_commander (moveit_commander.PlanningSceneInterface) – The moveit scene commander object.
  • collision_obj (moveit_collision_objects) – The moveit collision object.
  • is_known (bool, optional) – Bool specifying whether an object is known, by default False
  • is_attached (bool, optional) – Bool specifying whether an object is attached to something, by default False
  • timeout (int, optional) – The validation timeout time, by default 4
Returns:

  • bool – Boolean specifying whether the scene update was successful.
  • .. note:: – Note that attaching the box will remove it from known_objects.

Module contents

Module containing a number of functions that are used by the panda_autograsp package.

functions.download_model(model[, …]) This function downloads the Pretrained CNN models that are used in the panda_autograsp package, when they are not yet present on the system.
functions.list_files([path, exclude, …]) Returns a list of files that are present in a folder.
functions.yes_or_no(question[, add_options]) Simple yes or no prompt.
functions.draw_axis(img, corners, imgpts) Takes the corners in the chessboard (obtained using cv2.findChessboardCorners()) and axis points to draw a 3D axis.
moveit.get_trajectory_duration(trajectory) This function returns the duration of a given moveit trajectory.
moveit.plan_exists(plan) This function can be used to check if a plan trajectory was computed.
moveit.at_joint_target(current, desired, …) This function can be used to check if the move_group is already at the desired joint target.
moveit.add_collision_objects(…) Add the collision objects that are specified in the collision_cfg collections.OrderedDict to the moveit planner scene.
moveit.wait_for_state_update(…[, …]) Validate if collision object state change was successful.
conversions.transform_stamped_2_matrix(…) Generate 4x4 homogeneous transformation matrix from a transform_stamped msg.
conversions.pose_msg_stamped_2_matrix(pose_msg) Generate 4x4 homogeneous transformation matrix from a pose_stamped msg.