Sanjar reach manip

[SanjarNormuradov]

• add manipulability tests with different parameters (pod's position, motion planning parameters, rpy perturbations, etc).
• aurmr_perception/visualization.py is modified to visualize epick-end-effector with suction cup and metal cylinder.
• aurmr_tasks/src/aurmr_tasks/tahoma.py is modified to created more precise collsion scene for the pod in MoveIt, to account for different pod models, and to compute/execute motion plans for the given target pose/joint_values.
• robotiq/robotiq_epick_visualization/robotiq_epick_model_macro.xacro is modified to correspond the dimensions (collison objects) of the end-effector from meshes/visual/robotiq_epick_full.stl (which includes a suction cup and a metal cylinder to original epick end-effector). Those dimensions are explicitly defined in xacro files. tahoma_description/robots(or urdf)/pod.xacro is renamed to pod1.xacro (pod with 13x4 small bins).
• pod2.xacro file is created for the pod with 8x3 large bins.
• tahoma_description/urdf/tahoma.xacro is modified to explicitly define the dimensions and relative distances of coupling/connector_cover/robotiq_epick_end-effector and to include all of them (visual and collision objects) according to dae/stl files and real measurements. tahoma_moveit_config/config/omply_planning.yaml file has decreased 'longest_valid_segment_fraction' parameter from 0.01 to 0.001 to address thin bin walls (2 mm) in motion planning.
• tahoma_moveit_config/config/servo.yaml has collsion_check_rate of 200 instead of defalut 50 to avoid collision of the end-effector with bins when pre-grasp poses are very close to the bin's walls or when the robot is too close to the pod.
• tahoma_moveit_config/config/tahoma.srdf file is refined to exlcude obvious links from collision checking process

[nickswalker]

@SanjarNormuradov and I looked at this together today and removed some files from the pull request. The key changes to the moveit configuration and to the robot model are still unclear because the branch needs to be rebased on to the current main. Once that rebase is pushed here, we should run the system with the new planning configuration and robot model to confirm that the performance is as expected.

[nickswalker]

I was able to build the pod models with this version. Thanks! I left some comments about the structure of the xacro

[nickswalker]

It's odd for this file to have hardcoded behavior specific to particular pods, since anything that requires hard coding likely could've been included in the recipe as a parameter. I pushed a version of the shelf stack macro which takes the number of columns as a list argument for an idea of how that might work.

[nickswalker]

I see from the new pod_sizes.yaml that you've probably already discovered ${xacro.load_yaml ...}

It should be possible to rewrite this file so that it just loads all the parameters from the yaml file based on the passed in IDs.

[nickswalker]

By convention, xacro files in the robot directory of a package should produce a valid URDF when xacro is invoked on them. Macro definitions are expected to live under /urdf (strange convention, unless you think of them as the source code for the URDF, and the robot/ xacro as the (almost) executable form. So there shouldn't need to be a call_macro arg, it should be the only behavior.