Guided Insertion Operation
What you will learn
How to use several of pitasc’s most common skills, including linear movements, a guarded approach and different ways of using the slide skill. The application focus is the insertion of a T-piece into a fixture with guides.
Requirements
Hardware
Item |
Details |
|---|---|
3D printer |
For the parts listed in Provided 3D models |
Fasteners |
Screws ISO 4762 – M3x35 and nuts ISO 4032 – M3 (or comparable), 7 pcs each |
Robot |
Any pitasc-compatible robot with a force/torque sensor. This example uses a UR5e. |
Table (recommended) |
Welding table with 50 × 50 hole pattern, hole diameter 16 mm |
Positioning bolts |
16 mm, 4 – 6 pcs |
Supported grippers
Weiss WSG 50-110
Used in this example. Requires either:
F-WSG-FMFsensor fingers, or2 × custom fingers (model 05 below)
Robotiq 2F-140
Supported out of the box.
Robotiq 2F-85
Supported out of the box.
Franka Hand
Supported out of the box.
Important
The T-piece exists in a gripper-specific variant. Download the version that matches your hardware from T-piece - choose the variant for your gripper.
Provided 3D models
All models are hosted on GrabCAD and available as .step, .stl and
.sldprt.
Station and carrier
# |
Preview |
Part |
Downloads |
|---|---|---|---|
01 |
|
Lower section Note Print with the chamfer facing up. |
|
02 |
|
Upper section with guides Contains guide 1 (chamfered) and guide 2 (not chamfered). Note Print with the largest surface facing down. |
|
04 |
|
Workpiece carrier Defines the pick and place pose of the T-piece. Note Print with the chamfers facing up. |
|
05 |
|
Custom gripper fingers For the Weiss WSG 50-110 only. Note Print with the largest surface facing down. |
T-piece - choose the variant for your gripper
The T-piece (model 03). Print with the text facing up.
Gripper |
Downloads |
|---|---|
Weiss WSG 50-110 - custom gripper fingers |
|
Weiss WSG 50-110 - F-WSG-FMF sensor fingers |
|
Robotiq 2F-140 |
|
Robotiq 2F-85 |
|
Franka Hand |
Note
The first row reuses the gripper finger download links from the source document. If a dedicated T-piece variant for the custom fingers exists, replace those three links.
Setup
Step 1 - Print the parts
Print the lower and upper section, the workpiece carrier and the T-piece variant for your gripper (plus the custom gripper fingers if needed).
Warning
Tolerances were tested on a Bambu Lab X1 - adjust if needed. The fit between T-piece and guide should be between a clearance and a transition fit: it must be possible to insert the T-piece manually with little force.
Step 2 - Assemble the station
Assemble the lower and upper section using the screws and nuts.
Assembly of the 3D-printed parts with inserted T-piece. Guide 1 has chamfers to simplify insertion, guide 2 does not.
Step 3 - Clamp everything to the table
Clamp the assembled station and the workpiece carrier to your table.
Example configuration, allowing easy access to the T-piece and a minimal distance between the T-piece and both guides.
Step 4 - Position the T-piece
Place the T-piece in the workpiece carrier as shown below.
T-piece in the workpiece carrier.
Step 5 - Mount the gripper fingers
If needed, screw the custom 3D-printed gripper fingers onto the Weiss WSG 50-110 gripper.
How to solve this parkour task
The task is to insert and retrieve the T-piece into and out of both guides, as shown in the videos below.
Why the chamfer matters
Guides 1 and 2 differ only in the presence or absence of a chamfer at their opening. Chamfers simplify automated assembly by allowing the workpiece to self-centre, which reduces the precision required during assembly.
Try it yourself
Where to put your solution
Add your solution to:
.../pitasc_realworld_tutorial_p1/apps/parkour_1.xml
Then start RViz and the frame editor with:
roslaunch pitasc_realworld_tutorial_p1 bringup_ur5_parkour_1.launch
Increasing levels of assistance
The following sections progressively reveal more detail. Stop reading at whichever level still leaves you with an interesting challenge.
Level 1 – Rough steps - the six high-level actions
Level 2 – Example steps in detail - every individual motion, no code
Level 3 – Tips on using skills - which pitasc skills to reach for
Level 4 – Step-by-step sample solution - the full annotated XML
Level 1 – Rough steps
The sample solution comprises the following rough steps. Each step is implemented as a skill sequence containing several skills:
Pick up the workpiece from the workpiece carrier.
Insert the workpiece into guide 1.
Retrieve the workpiece from guide 1.
Insert the workpiece into guide 2.
Retrieve the workpiece from guide 2.
Place the workpiece back on the workpiece carrier.
Level 2 – Example steps in detail
Note
For safety reasons, each step begins and ends with the robot moving to its home pose by linear movement.
Step 1 - Pick up
Move to home pose (position controlled)
Move to a position above the workpiece carrier (position controlled)
Lower the end effector to the gripping position (position controlled)
Grip the T-piece (command to your specific gripper)
Push down on the T-piece to ensure a safe grip (force controlled - only needed for flat gripper fingers)
Move upwards (position controlled)
Move to home pose (position controlled)
Step 2 - Insert into guide 1 (chamfered)
Move to home pose (position controlled)
Move to a position above the contact position (position controlled)
Move to a position close to the guide (position controlled)
Establish contact between T-piece and station (force controlled)
Slide the T-piece into the guide until it is fully inside (force controlled)
Slide the T-piece until it hits the guide end (force controlled)
Open the gripper (command to your specific gripper)
Move to home pose (position controlled)
Step 3 - Retrieve from guide 1
Move to home pose (position controlled)
Move to a position above the workpiece (position controlled)
Lower the end effector to the gripping position (position controlled)
Grip the T-piece (command to your specific gripper)
Push down on the T-piece to ensure a safe grip (force controlled - only needed for flat gripper fingers)
Slide the T-piece out (force controlled)
Move up (position controlled)
Move to home pose (position controlled)
Step 4 - Insert into guide 2 (not chamfered)
Move to home pose (position controlled)
Move to a position above the contact position (position controlled)
Move to a position close to the guide (position controlled)
Tilt the T-piece so that the bottom left corner is exposed (position controlled)
Establish contact between T-piece and station (force controlled)
Apply force so that the bottom left corner of the T-piece is pushed into the bottom left corner of the guide (force controlled)
Apply force in the direction of the guide and align the T-piece with the guide (force and position controlled)
Slide the T-piece into the guide (force controlled)
Open the gripper (command to your specific gripper)
Move to home pose (position controlled)
Tip
Steps 4.4 – 4.7 replace the self-centring that the chamfer provides in guide 1. Tilting and corner-seating creates a known contact state from which the insertion direction is unambiguous.
Step 5 - Retrieve from guide 2
Move to home pose (position controlled)
Move to a position above the workpiece (position controlled)
Lower the end effector to the gripping position (position controlled)
Grip the T-piece (command to your specific gripper)
Push down on the T-piece to ensure a safe grip (force controlled - only needed for flat gripper fingers)
Slide the T-piece out (force controlled)
Move up (position controlled)
Move to home pose (position controlled)
Step 6 - Place in the workpiece carrier
Move to home pose (position controlled)
Move to a position above the workpiece carrier (position controlled)
Establish contact between T-piece and workpiece carrier (force controlled)
Open the gripper (command to your specific gripper)
Move upwards (position controlled)
Level 3 – Tips on using skills
Goal |
Recommended approach |
|---|---|
Establish contact with the surroundings |
Use a guarded approach. |
Predictable linear motion with minimal jerk |
Use |
Follow a wall or guide under constant force |
Use the pitasc slide skill. |
End a skill on a condition |
Attach monitors to react to a force threshold or travelled distance. |
Danger
Tare the force/torque sensor before every force-controlled skill. This is the single most important step for reliable real-world operation - an untared sensor makes the measured contact force meaningless and can damage the workpiece or the station. See the code snippets below for how taring is invoked in practice.
Tip
Add user confirms between skills. This makes it much easier to spot problems and abort execution safely while developing.
Level 4 – Step-by-step sample solution
Preamble
The preamble handles all imports of relevant models and configures the application environment, in which every component (robots, sensors, actuators) must be specified in order to be controlled later on. It also defines a boolean parameter to activate or deactivate all user confirmations at once.
0_preamble.xml
<?xml version="1.0" encoding="UTF-8"?>
<pitasc>
<models>
<!-- Include relevant models -->
<include package="pitasc_library" file="models/pitasc.xml"/>
<include package="pitasc_library" file="models/skills.xml"/>
<include package="pitasc_library" file="utils/gravity_compensator.xml"/>
<include package="pitasc_library" file="universal_robots/ur.xml"/>
<include package="pitasc_library" file="weiss_robotics/kms40.xml"/>
<include package="pitasc_weiss_wsg" file="models/wsg50.xml"/>
<include package="pitasc_library" file="frame_editor/frame_editor.xml" />
</models>
<!-- Create an application -->
<clone prototype="project">
<!-- Parameter to ignore all user confirms. If set to True, all user
confirms are skipped. If set to False, all user confirms are active -->
<type id="ignore_uc" prototype="bool_parameter">
<meta>
<member id="description">Parameter to ignore user confirms (uc) and fire the (success) event immediately</member>
<member id="visibility">basic</member>
</meta>
<data>False</data>
</type>
<!-- Configure pitasc (set update rate, relevant for controllers)-->
<member id="configuration">
<clone id="configuration" prototype="default_configuration">
<member id="update_rate">250</member>
</clone>
</member>
<!-- Set up environment -->
<member id="environment">
<!-- Specify and configure robot -->
<clone id="ur5" prototype="robot_ur5">
<member id="robot_driver.max_velocity">0.4</member>
<member id="robot_driver.max_acceleration">1.0</member>
<member id="robot_driver.topic_in">ur5/joint_states</member>
<member id="robot_driver.topic_out">ur5/joint_vel</member>
<member id="robot_driver.urdf_key">ur5/robot_description</member>
<member id="urdf_prefix">ur5_</member>
<member id="components">
<!-- Specify used force/torque sensor (in this example the robots internal f/t-sensor) -->
<clone id="force_sensor" prototype="weiss_kms40">
<member id="wrench_topic">/wrench_compensated</member>
<member id="namespace">/ur5</member>
</clone>
<!-- Specify used gripper -->
<clone id="gripper" prototype="weiss_wsg50">
<member id="namespace">/ur5/wsg50</member>
</clone>
</member>
</clone>
</member>
<member id="applications">
<!-- ######################################### -->
<!-- -->
<!-- Skill sequences need to be specified here -->
<!-- -->
<!-- ######################################### -->
</member>
</clone>
</pitasc>
Homing sequence for the gripper
This short skill sequence commands the WSG 50-110 gripper to run its homing sequence.
0.1_home_gripper.xml
<!-- Skill sequence for homing the gripper -->
<clone id="Home gripper" prototype="skill_sequence">
<!-- Specify which robot will be controlled -->
<member id="robot" reference_id="environment.ur5"/>
<!-- The skills that will be carried out by the specified robot -->
<member id="skills">
<!-- Wait, until transformations are available -->
<clone prototype="skill_wait_for_transform">
<member id="frame">ur5_weiss_wsg50_grip_center</member>
<member id="parent_frame">world</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Home gripper?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Home the gripper.
Caution: the gripper will open completely -->
<clone prototype="weiss_wsg50_homing" />
</member>
</clone>
Step 1: Pick up the workpiece from the carrier
Picks up the T-piece and carries it to the robot’s home pose.
1_pick.xml
<!-- Skill sequence for picking up the T-piece from the workpiece
holder -->
<clone id="Pick up T-piece from workpiece holder" prototype="skill_sequence">
<!-- Specify which robot will be controlled -->
<member id="robot" reference_id="environment.ur5"/>
<!-- The skills that will be carried out by the specified robot -->
<member id="skills">
<!-- Wait, until transformations are available -->
<clone prototype="skill_wait_for_transform">
<member id="frame">ur5_weiss_wsg50_grip_center</member>
<member id="parent_frame">world</member>
</clone>
<!-- Tare force/torque sensor output -->
<clone prototype="skill_tare_gravity_compensator">
<member id="duration">0.1</member>
<member id="namespace">ur5</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Open gripper and move robot to home pos?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move gripper fingers to defined position -->
<clone id="release T-piece" prototype="weiss_wsg50_position">
<member id="position">60.0</member>
<member id="speed">50.0</member>
</clone>
<!-- Move end effector to home position -->
<clone prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">home_pos</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move robot to safe approach pos above T-piece?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move end effector to a position in a safe distance above workiece holder -->
<clone prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">t_piece_safe_approach_pos</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move robot to grasp position?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Lower the end effector to the grasp position -->
<clone prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">t_piece_grasp_pos</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Grip T-piece?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Perform gripping with set speed -->
<clone prototype="weiss_wsg50_grip">
<member id="position">30.0</member>
<member id="speed">50.0</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Push down on T-piece?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Push down onto T-piece to deepen grip -->
<clone id="ensure full grip on T-piece" prototype="skill_push_duration">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="force_frame">ur5_weiss_wsg50_grip_center</member>
<member id="duration">1.0</member>
<member id="target_forces">0.0, 0.0, 15.0</member>
<member id="axes">x, y, z</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Lift T-piece?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move the end effector to a position with a relative
offset in vertical direction -->
<clone prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">t_piece_grasp_pos</member>
<member id="axes">x, y, z, a, b, c</member>
<member id="target_offsets">0.0, 0.0, -0.05, 0.0, 0.0, 0.0</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move T-piece to home pos?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Return to home pose while holding the T-piece -->
<clone prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">home_pos</member>
</clone>
</member>
</clone>
Step 2: Insert into guide 1
Because the opening of guide 1 is chamfered, the T-piece centres itself (within limits). The position of the T-piece relative to the opening is therefore far less critical than for guide 2.
2_insert_guide_1.xml
<!-- Skill sequence for inserting the T-piece in the guide with
champfers -->
<clone id="Insert T-piece (DfA conform guide)" prototype="skill_sequence">
<!-- Specify which robot will be controlled -->
<member id="robot" reference_id="environment.ur5"/>
<!-- The skills that will be carried out by the specified robot -->
<member id="skills">
<!-- Wait, until transformations are available -->
<clone prototype="skill_wait_for_transform">
<member id="frame">ur5_weiss_wsg50_grip_center</member>
<member id="parent_frame">world</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move robot to home pos?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move end effector to home position -->
<clone id="move robot to home pos" prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">home_pos</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move robot to safe approach pos?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move end effector to position closer to guide while
keeping a safe distance -->
<clone id="move to safe approach pos" prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">dfa_guide_safe_approach_pos</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move robot to pre insertion pos?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Lower end effector with T-piece to the height of guide
opening -->
<clone id="move to close approach pos" prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">dfa_guide_close_approach_pos</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move robot to pre contact pos?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Slowly move end effector with T-piece close to guide
opening, without physical contact between the workpiece and
the guide -->
<clone id="move end effector to pre contact position" prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">dfa_guide_pre_contact_pos</member>
<member id="max_linear_velocity">0.01</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Establish contact?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Tare force/torque sensor output -->
<clone id="tare ft sensor" prototype="skill_tare_gravity_compensator">
<member id="duration">0.1</member>
<member id="namespace">ur5</member>
</clone>
<!-- Move T-piece to guide opening until physical contact
has been established -->
<clone id="establish contact" prototype="skill_guarded_approach">
<member id="max_forces">5.0</member>
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="control_frame">table</member>
<member id="axes">y</member>
<member id="velocities">-0.005</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Slide with 15 N?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Slide the T-piece until it is completely inserted -->
<clone id="Guarded slide with 15 N" prototype="skill_slide">
<member id="control_frame">table</member>
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="move_axes">x, y, z, a, b, c</member>
<member id="velocities">0.0, -0.005, 0.0, 0.0, 0.0, 0.0</member>
<member id="force_axes">x, y, z, a, b, c</member>
<member id="target_forces">0.0, -12.5, 0.0, 0.0, 0.0, 0.0</member>
<member id="monitors">
<clone prototype="monitor_relative_distance">
<member id="event">succeeded</member>
<member id="reference_frame">table</member>
<member id="frame">ur5_weiss_wsg50_grip_center</member>
<member id="coordinates">y</member>
<member id="distance_coordinates">y</member>
<member id="distances">0.05</member>
<member id="operator">absolute_greater</member>
</clone>
</member>
</clone>
<!-- Slide the T-piece inside the guide until it touches the
limit of the guide with constant speed. The termination
criterion is the high forces that occur when the rear wall
of the guide is reached. -->
<clone id="Slide until guide end is reached" prototype="skill_guarded_slide">
<member id="control_frame">table</member>
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="max_forces">25.0</member>
<member id="operator">absolute_greater</member>
<member id="move_axes">y</member>
<member id="velocities">-0.005</member>
<member id="force_axes">x, y, z, a, b, c</member>
<member id="target_forces">0.0, -15.0, 0.0, 0.0, 0.0, 0.0</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Open gripper?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Open gripper to set position with set speed -->
<clone id="release T-piece" prototype="weiss_wsg50_release">
<member id="position">60.0</member>
<member id="speed">50.0</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move robot upwards?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Because the position of the end effector is not fully
predictable when using force controlled motions, the gripper
is moved upwards relatively -->
<clone id="move end effector upwards" prototype="skill_relative_lin">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="axes">x, y, z, a, b, c</member>
<member id="target_offsets">0.0, 0.0, -0.05, 0.0, 0.0, 0.0</member>
<member id="max_linear_velocity">0.05</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move robot to home pos?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move end effector back to home position -->
<clone id="move end effector to home pos" prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">home_pos</member>
</clone>
</member>
</clone>
Step 3: Retrieve from guide 1
The pose at which the gripper held the inserted workpiece was saved. The following code slides the T-piece out of the guide and carries it back to the home pose.
3_retrieve_guide_1.xml
<!-- Skill sequence for retrieving the T-piece from the guide with
champfers -->
<clone id="Retrieve T-piece (DfA conform guide)" prototype="skill_sequence">
<!-- Specify which robot will be controlled -->
<member id="robot" reference_id="environment.ur5"/>
<!-- The skills that will be carried out by the specified robot -->
<member id="skills">
<!-- Wait, until transformations are available -->
<clone prototype="skill_wait_for_transform">
<member id="frame">ur5_weiss_wsg50_grip_center</member>
<member id="parent_frame">world</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move robot home?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move end effector to home position -->
<clone id="" prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">home_pos</member>
</clone>
<!-- Make sure that gripper is opened -->
<clone id="Open gripper" prototype="weiss_wsg50_position">
<member id="position">60.0</member>
<member id="speed">50.0</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move to position above T-piece?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move end effector to position above gripping
position -->
<clone prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">dfa_guide_grasp_pos</member>
<member id="axes">x, y, z, a, b, c</member>
<member id="target_offsets">0.0, 0.0, -0.05, 0.0, 0.0, 0.0</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move to grasp position?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move end effector to gripping position -->
<clone prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">dfa_guide_grasp_pos</member>
<member id="axes">x, y, z, a, b, c</member>
</clone>
<!-- Tare the force/torque sensor -->
<clone prototype="skill_tare_gravity_compensator">
<member id="duration">0.1</member>
<member id="namespace">ur5</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Close gripper?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Close fingers to grip T-piece -->
<clone prototype="weiss_wsg50_grip">
<member id="position">30.0</member>
<member id="speed">50.0</member>
</clone>
<!-- Push down on T-piece to ensure safe grip -->
<clone id="ensure full grip on T-piece" prototype="skill_push_duration">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="force_frame">ur5_weiss_wsg50_grip_center</member>
<member id="duration">1.0</member>
<member id="target_forces">0.0, 0.0, 15.0</member>
<member id="axes">x, y, z</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Slide T-piece out of guide?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Slide T-piece out of guide for a set distance -->
<clone id="Slide with 15 N" prototype="skill_slide">
<member id="control_frame">table</member>
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="move_axes">x, y, z, a, b, c</member>
<member id="velocities">0.0, 0.005, 0.0, 0.0, 0.0, 0.0</member>
<member id="force_axes">x, y, z, a, b, c</member>
<member id="target_forces">0.0, 12.5, 0.0, 0.0, 0.0, 0.0</member>
<member id="monitors">
<clone prototype="monitor_relative_distance">
<member id="event">succeeded</member>
<member id="reference_frame">table</member>
<member id="frame">ur5_weiss_wsg50_grip_center</member>
<member id="coordinates">y</member>
<member id="distance_coordinates">y</member>
<member id="distances">0.10</member>
<member id="operator">absolute_greater</member>
</clone>
</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move end effector up?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move the T-piece away from the guide to prevent
collision when returning to home pos -->
<clone prototype="skill_relative_lin">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="axes">x, y, z, a, b, c</member>
<member id="target_offsets">0.05, 0.0, -0.05, 0.0, 0.0, 0.0</member>
<member id="max_linear_velocity">0.05</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move robot to home pos?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move end effector back to home pos -->
<clone prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">home_pos</member>
</clone>
</member>
</clone>
Step 4: Insert into guide 2
While inserting into guide 2 appears to be the same task as guide 1, the lack of a chamfer proves to be an obstacle. The T-piece is therefore tilted and pushed into one corner of the guide to achieve a form of mutual centring.
4_insert_guide_2.xml
<!-- Skill sequence for inserting the T-piece in the guide without
champfers -->
<clone id="Insert T-piece (plain guide)" prototype="skill_sequence">
<!-- Specify which robot will be controlled -->
<member id="robot" reference_id="environment.ur5"/>
<!-- The skills that will be carried out by the specified robot -->
<member id="skills">
<!-- Wait, until transformations are available -->
<clone prototype="skill_wait_for_transform">
<member id="frame">ur5_weiss_wsg50_grip_center</member>
<member id="parent_frame">world</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move robot to home pos?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move end effector to home position -->
<clone id="move robot to home pos" prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">home_pos</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move robot to safe approach pos?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move end effector to safe position closer to guide
while holding T-piece -->
<clone id="move to safe approach pos" prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">plain_guide_safe_approach_pos</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move robot to close approach pos?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Lower T-piece to guide height -->
<clone id="move to close approach pos" prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">plain_guide_close_approach_pos</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move robot to pre contact pos?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move T-piece closer to guide without establishing
contact -->
<clone id="move end effector to pre contact position" prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">plain_guide_pre_contact_pos</member>
<member id="max_linear_velocity">0.01</member>
</clone>
<!-- Tilt workpiece to extend lower left corner -->
<clone prototype="skill_relative_lin">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="axes">x, y, z, a, b, c</member>
<member id="target_offsets">0.0, 0.0, -0.002, 0.0, -0.070, 0.070</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Establish contact?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Tare force/torque sensor output -->
<clone id="tare ft sensor" prototype="skill_tare_gravity_compensator">
<member id="duration">0.1</member>
<member id="namespace">ur5</member>
</clone>
<!-- Move T-piece closer to guide until contact is established -->
<clone id="establish contact" prototype="skill_guarded_approach">
<member id="max_forces">5.0</member>
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="control_frame">table</member>
<member id="axes">y</member>
<member id="velocities">0.005</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Push into bottom left corner?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Press bottom left corner of T-piece into bottom left
corner of guide -->
<clone id="push T-piece into bottom left corner of guide" prototype="skill_push_duration">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="force_frame">table</member>
<member id="duration">2.0</member>
<member id="target_forces">-10.0, 10.0, -10.0</member>
<member id="axes">x, y, z</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Insert?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Insert by simultaneously aligning T-piece orientation
with guide and pressing into it -->
<clone prototype="skill_insert">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">plain_guide_pre_contact_pos</member>
<member id="force_axes">x, y, z</member>
<member id="track_axes">a,b,c</member>
<member id="track_offsets">0,0,0</member>
<member id="target_forces">-15.0, 0.0, 0.0</member>
<member id="monitors">
<clone prototype="monitor_duration">
<member id="event">succeeded</member>
<member id="duration">2.0</member>
</clone>
</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Slide with 15 N?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Make sure T-piece is further inserted by sliding it
into guide -->
<clone id="Slide with 15 N" prototype="skill_slide">
<member id="control_frame">table</member>
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="move_axes">x, y, z, a, b, c</member>
<member id="velocities">0.0, 0.005, 0.0, 0.0, 0.0, 0.0</member>
<member id="force_axes">x, y, z, a, b, c</member>
<member id="target_forces">0.0, 15.0, 3.0, 0.0, 0.0, 0.0</member>
<member id="monitors">
<clone prototype="monitor_duration">
<member id="event">succeeded</member>
<member id="duration">2.0</member>
</clone>
</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Press down on T-piece?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Ensure full grip by pressing down -->
<clone id="press down on T-piece" prototype="skill_push_duration">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="force_frame">ur5_weiss_wsg50_grip_center</member>
<member id="duration">1.0</member>
<member id="target_forces">0.0, 0.0, 10.0</member>
<member id="axes">x, y, z</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Cycle through stress relief and slide?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Sub-sequence alternating between sliding and reducing
forces to prevent jamming of the T-piece -->
<clone prototype="skill_sequence">
<member id="robot" reference_id="environment.ur5"/>
<member id="skills">
<!-- Reduce forces -->
<clone id="reduce_forces" prototype="skill_apply_force">
<member id="force_frame">table</member>
<member id="axes">x, y, z</member>
<member id="target_forces">0.0, 0.0, 0.0</member>
<member id="monitors">
<clone prototype="monitor_duration">
<member id="event">succeeded</member>
<member id="duration">1.0</member>
</clone>
</member>
</clone>
<!-- Continue inserting the T-piece for specified
duration -->
<clone prototype="skill_insert">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">plain_guide_pre_contact_pos</member>
<member id="force_axes">x, z</member>
<member id="track_axes">a,b,c</member>
<member id="track_offsets">0,0,0</member>
<member id="target_forces">-25.0, 5.0</member>
<member id="transitions">
<clone prototype="transition">
<member id="event">succeeded</member>
<member id="target">reduce_forces</member>
</clone>
</member>
<member id="monitors">
<clone prototype="monitor_duration">
<member id="event">succeeded</member>
<member id="duration">1.0</member>
</clone>
</member>
</clone>
</member>
<!-- Monitor for the entire sub-sequence. Once the
requirement is met, the sequence will end -->
<member id="monitors">
<!-- Once the endeffector has reached a specified
absolute distance from when the sub-sequence
started, the sub-sequence will end -->
<clone prototype="monitor_relative_distance">
<member id="event">succeeded</member>
<member id="reference_frame">table</member>
<member id="frame">ur5_weiss_wsg50_grip_center</member>
<member id="coordinates">y</member>
<member id="distance_coordinates">y</member>
<member id="distances">0.07</member>
<member id="operator">absolute_greater</member>
</clone>
</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Insert for 10 seconds?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Ensure safe gripp before continuing to slide -->
<clone id="ensure full gripping contact" prototype="skill_push_duration">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="force_frame">ur5_weiss_wsg50_grip_center</member>
<member id="duration">1.0</member>
<member id="target_forces">0.0, 0.0, 15.0</member>
<member id="axes">x, y, z</member>
</clone>
<!-- Slide for a set time so that T-piece reaches rear wall
of guide -->
<clone prototype="skill_insert">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">plain_guide_pre_contact_pos</member>
<member id="force_axes">x, y, z</member>
<member id="track_axes">a,b,c</member>
<member id="track_offsets">0,0,0</member>
<member id="target_forces">-25.0, 0.0, 3.0</member>
<member id="monitors">
<clone prototype="monitor_duration">
<member id="event">succeeded</member>
<member id="duration">3.0</member>
</clone>
</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Open gripper?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Open gripper until specified position is reached -->
<clone id="release T-piece" prototype="weiss_wsg50_release">
<member id="position">60.0</member>
<member id="speed">50.0</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move robot to home pos?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move robot back to home position -->
<clone id="return home" prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">home_pos</member>
</clone>
</member>
</clone>
Step 5: Retrieve from guide 2
Similar to the retrieval from guide 1. The main difference is the change of direction for sliding and for moving the T-piece away from the guide.
5_retrieve_guide_2.xml
<!-- Skill sequence for retrieving the T-piece from the guide with
champfers -->
<clone id="Retrieve T-piece (plain guide)" prototype="skill_sequence">
<!-- Specify which robot will be controlled -->
<member id="robot" reference_id="environment.ur5"/>
<!-- The skills that will be carried out by the specified robot -->
<member id="skills">
<!-- Wait, until transformations are available -->
<clone prototype="skill_wait_for_transform">
<member id="frame">ur5_weiss_wsg50_grip_center</member>
<member id="parent_frame">world</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move robot home?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move end effector to home pos -->
<clone id="move to home pos" prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">home_pos</member>
</clone>
<!-- Make sure gripper is open -->
<clone id="open gripper" prototype="weiss_wsg50_position">
<member id="position">60.0</member>
<member id="speed">50.0</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move to position above T-piece?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move end effector to position above T-piece -->
<clone prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">plain_guide_grasp_pos</member>
<member id="axes">x, y, z, a, b, c</member>
<member id="target_offsets">0.0, 0.0, -0.05, 0.0, 0.0, 0.0</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move to grasp position?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move end effector to gripping position -->
<clone prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">plain_guide_grasp_pos</member>
<member id="axes">x, y, z, a, b, c</member>
</clone>
<!-- Tare force/torque sensor output -->
<clone prototype="skill_tare_gravity_compensator">
<member id="duration">0.1</member>
<member id="namespace">ur5</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Close gripper?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Gripp T-piece -->
<clone prototype="weiss_wsg50_grip">
<member id="position">30.0</member>
<member id="speed">50.0</member>
</clone>
<!-- Push down on T-piece to ensure full grip -->
<clone id="ensure full grip on T-piece" prototype="skill_push_duration">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="force_frame">ur5_weiss_wsg50_grip_center</member>
<member id="duration">1.0</member>
<member id="target_forces">0.0, 0.0, 15.0</member>
<member id="axes">x, y, z</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Slide T-piece out of guide?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Slide T-piece completely out of guide -->
<clone id="Slide with 15 N" prototype="skill_slide">
<member id="control_frame">table</member>
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="move_axes">x, y, z, a, b, c</member>
<member id="velocities">0.0, -0.005, 0.0, 0.0, 0.0, 0.0</member>
<member id="force_axes">x, y, z, a, b, c</member>
<member id="target_forces">0.0, 12.5, 0.0, 0.0, 0.0, 0.0</member>
<member id="monitors">
<clone prototype="monitor_relative_distance">
<member id="event">succeeded</member>
<member id="reference_frame">table</member>
<member id="frame">ur5_weiss_wsg50_grip_center</member>
<member id="coordinates">y</member>
<member id="distance_coordinates">y</member>
<member id="distances">0.10</member>
<member id="operator">absolute_greater</member>
</clone>
</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move end effector up?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move T-piece away from guide and upwards -->
<clone prototype="skill_relative_lin">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="axes">x, y, z, a, b, c</member>
<member id="target_offsets">0.05, 0.0, -0.05, 0.0, 0.0, 0.0</member>
<member id="max_linear_velocity">0.05</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move robot to home pos?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move end effector back to home pos -->
<clone prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">home_pos</member>
</clone>
</member>
</clone>
Step 6: Place in the workpiece carrier
The final step places the T-piece back in the carrier. To prevent an unwanted collision in case the workpiece slipped during retrieval, the robot first moves to a position just above the carrier, then gently establishes contact and pushes the workpiece into the recess.
6_place.xml
<!-- Skill sequence for placing the T-piece in workpiece holder -->
<clone id="Place T-piece in workpiece holder" prototype="skill_sequence">
<!-- Define which robot will be controlled -->
<member id="robot" reference_id="environment.ur5"/>
<!-- The skills that will be carried out by the specified robot -->
<member id="skills">
<!-- Wait, until transformations are available -->
<clone prototype="skill_wait_for_transform">
<member id="frame">ur5_weiss_wsg50_grip_center</member>
<member id="parent_frame">world</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move robot to home pos?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move end effector to home position -->
<clone prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">home_pos</member>
</clone>
<!-- Tare force/torque sensor output -->
<clone prototype="skill_tare_gravity_compensator">
<member id="duration">0.1</member>
<member id="namespace">ur5</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move robot to safe approach pos above T-piece?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move end effector holding the T-piece to safe position above
workpiece holder -->
<clone prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">t_piece_safe_approach_pos</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move robot to pre placing position?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move end effector close to workiece holder without
establishing contact between T-piece and workpiece
holder -->
<clone prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">t_piece_grasp_pos</member>
<member id="axes">x, y, z, a, b, c</member>
<member id="target_offsets">0.0, 0.0, -0.01, 0.0, 0.0, 0.0</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Place down T-piece?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move T-piece in direction of workpiece holder until
defined force is reached to establish contact -->
<clone id="establish contact" prototype="skill_guarded_approach">
<member id="max_forces">5.0</member>
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="control_frame">ur5_weiss_wsg50_grip_center</member>
<member id="axes">z</member>
<member id="velocities">0.005</member>
</clone>
<!-- Push down on T-piece to make sure it is inserted into
recess in workpiece holder -->
<clone id="place T-piece" prototype="skill_push_duration">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="force_frame">ur5_weiss_wsg50_grip_center</member>
<member id="duration">1.0</member>
<member id="target_forces">0.0, 0.0, 5.0</member>
<member id="axes">x, y, z</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Open gripper?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Open gripper to set position with set speed -->
<clone id="release T-piece" prototype="weiss_wsg50_release">
<member id="position">60.0</member>
<member id="speed">50.0</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move robot upwards?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move end effector upwards -->
<clone id="move end effector_upwards" prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">t_piece_grasp_pos</member>
<member id="axes">x, y, z, a, b, c</member>
<member id="target_offsets">0.0, 0.0, -0.05, 0.0, 0.0, 0.0</member>
</clone>
<clone prototype="skill_user_confirm">
<member id="title">Confirmation required</member>
<member id="text">Move robot to home pos?</member>
<member id="ignore" reference_id="ignore_uc" />
</clone>
<!-- Move end effector back to home position -->
<clone id="return to home pos" prototype="skill_lin_time_optimal_ruckig">
<member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
<member id="target_frame">home_pos</member>
</clone>
</member>
</clone>
Complete sample solution
For completeness, the full application is shown below. It includes all previously described steps, from homing the gripper to placing the T-piece back in the workpiece carrier.
7_complete_application.xml - full application
1<?xml version="1.0" encoding="UTF-8"?>
2
3<pitasc>
4 <models>
5 <!-- Include relevant models -->
6 <include package="pitasc_library" file="models/pitasc.xml"/>
7 <include package="pitasc_library" file="models/skills.xml"/>
8 <include package="pitasc_library" file="utils/gravity_compensator.xml"/>
9 <include package="pitasc_library" file="universal_robots/ur.xml"/>
10 <include package="pitasc_library" file="weiss_robotics/kms40.xml"/>
11 <include package="pitasc_weiss_wsg" file="models/wsg50.xml"/>
12 <include package="pitasc_library" file="frame_editor/frame_editor.xml" />
13 </models>
14
15 <!-- Create an application -->
16 <clone prototype="project">
17
18 <!-- Parameter to ignore all user confirms. If set to True, all user
19 confirms are skipped. If set to False, all user confirms are active -->
20 <type id="ignore_uc" prototype="bool_parameter">
21 <meta>
22 <member id="description">Parameter to ignore user confirms (uc) and fire the (success) event immediately</member>
23 <member id="visibility">basic</member>
24 </meta>
25 <data>False</data>
26 </type>
27
28 <!-- Configure pitasc (set update rate, relevant for controllers)-->
29 <member id="configuration">
30 <clone id="configuration" prototype="default_configuration">
31 <member id="update_rate">250</member>
32 </clone>
33 </member>
34
35 <!-- Set up environment -->
36 <member id="environment">
37 <!-- Specify and configure robot -->
38 <clone id="ur5" prototype="robot_ur5">
39 <member id="robot_driver.max_velocity">0.4</member>
40 <member id="robot_driver.max_acceleration">1.0</member>
41 <member id="robot_driver.topic_in">ur5/joint_states</member>
42 <member id="robot_driver.topic_out">ur5/joint_vel</member>
43 <member id="robot_driver.urdf_key">ur5/robot_description</member>
44 <member id="urdf_prefix">ur5_</member>
45 <member id="components">
46 <!-- Specify used force/torque sensor (in this example the robots internal f/t-sensor) -->
47 <clone id="force_sensor" prototype="weiss_kms40">
48 <member id="wrench_topic">/wrench_compensated</member>
49 <member id="namespace">/ur5</member>
50 </clone>
51 <!-- Specify used gripper -->
52 <clone id="gripper" prototype="weiss_wsg50">
53 <member id="namespace">/ur5/wsg50</member>
54 </clone>
55 </member>
56 </clone>
57 </member>
58
59 <!-- "Applications" contains the usable skill sequences. Once this xml has
60 been loaded with piteacher, the skill sequences can be started by
61 pressing the corresponding button showing their id -->
62 <member id="applications">
63
64 <!-- Skill sequence for homing the gripper -->
65 <clone id="Home gripper" prototype="skill_sequence">
66 <!-- Specify which robot will be controlled -->
67 <member id="robot" reference_id="environment.ur5"/>
68 <!-- The skills that will be carried out by the specified robot -->
69 <member id="skills">
70
71 <!-- Wait, until transformations are available -->
72 <clone prototype="skill_wait_for_transform">
73 <member id="frame">ur5_weiss_wsg50_grip_center</member>
74 <member id="parent_frame">world</member>
75 </clone>
76
77 <clone prototype="skill_user_confirm">
78 <member id="title">Confirmation required</member>
79 <member id="text">Home gripper?</member>
80 <member id="ignore" reference_id="ignore_uc" />
81 </clone>
82
83 <!-- Home the gripper.
84 Caution: the gripper will open completely -->
85 <clone prototype="weiss_wsg50_homing" />
86 </member>
87 </clone>
88
89 <!-- Skill sequence for picking up the T-piece from the workpiece
90 holder -->
91 <clone id="Pick up T-piece from workpiece holder" prototype="skill_sequence">
92 <!-- Specify which robot will be controlled -->
93 <member id="robot" reference_id="environment.ur5"/>
94 <!-- The skills that will be carried out by the specified robot -->
95 <member id="skills">
96
97 <!-- Wait, until transformations are available -->
98 <clone prototype="skill_wait_for_transform">
99 <member id="frame">ur5_weiss_wsg50_grip_center</member>
100 <member id="parent_frame">world</member>
101 </clone>
102
103 <!-- Tare force/torque sensor output -->
104 <clone prototype="skill_tare_gravity_compensator">
105 <member id="duration">0.1</member>
106 <member id="namespace">ur5</member>
107 </clone>
108
109 <clone prototype="skill_user_confirm">
110 <member id="title">Confirmation required</member>
111 <member id="text">Open gripper and move robot to home pos?</member>
112 <member id="ignore" reference_id="ignore_uc" />
113 </clone>
114
115 <!-- Move gripper fingers to defined position -->
116 <clone id="release T-piece" prototype="weiss_wsg50_position">
117 <member id="position">60.0</member>
118 <member id="speed">50.0</member>
119 </clone>
120
121 <!-- Move end effector to home position -->
122 <clone prototype="skill_lin_time_optimal_ruckig">
123 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
124 <member id="target_frame">home_pos</member>
125 </clone>
126
127 <clone prototype="skill_user_confirm">
128 <member id="title">Confirmation required</member>
129 <member id="text">Move robot to safe approach pos above T-piece?</member>
130 <member id="ignore" reference_id="ignore_uc" />
131 </clone>
132
133 <!-- Move end effector to a position in a safe distance above workiece holder -->
134 <clone prototype="skill_lin_time_optimal_ruckig">
135 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
136 <member id="target_frame">t_piece_safe_approach_pos</member>
137 </clone>
138
139 <clone prototype="skill_user_confirm">
140 <member id="title">Confirmation required</member>
141 <member id="text">Move robot to grasp position?</member>
142 <member id="ignore" reference_id="ignore_uc" />
143 </clone>
144
145 <!-- Lower the end effector to the grasp position -->
146 <clone prototype="skill_lin_time_optimal_ruckig">
147 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
148 <member id="target_frame">t_piece_grasp_pos</member>
149 </clone>
150
151 <clone prototype="skill_user_confirm">
152 <member id="title">Confirmation required</member>
153 <member id="text">Grip T-piece?</member>
154 <member id="ignore" reference_id="ignore_uc" />
155 </clone>
156
157 <!-- Perform gripping with set speed -->
158 <clone prototype="weiss_wsg50_grip">
159 <member id="position">30.0</member>
160 <member id="speed">50.0</member>
161 </clone>
162
163 <clone prototype="skill_user_confirm">
164 <member id="title">Confirmation required</member>
165 <member id="text">Push down on T-piece?</member>
166 <member id="ignore" reference_id="ignore_uc" />
167 </clone>
168
169 <!-- Push down onto T-piece to deepen grip -->
170 <clone id="ensure full grip on T-piece" prototype="skill_push_duration">
171 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
172 <member id="force_frame">ur5_weiss_wsg50_grip_center</member>
173 <member id="duration">1.0</member>
174 <member id="target_forces">0.0, 0.0, 15.0</member>
175 <member id="axes">x, y, z</member>
176 </clone>
177
178 <clone prototype="skill_user_confirm">
179 <member id="title">Confirmation required</member>
180 <member id="text">Lift T-piece?</member>
181 <member id="ignore" reference_id="ignore_uc" />
182 </clone>
183
184 <!-- Move the end effector to a position with a relative
185 offset in vertical direction -->
186 <clone prototype="skill_lin_time_optimal_ruckig">
187 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
188 <member id="target_frame">t_piece_grasp_pos</member>
189 <member id="axes">x, y, z, a, b, c</member>
190 <member id="target_offsets">0.0, 0.0, -0.05, 0.0, 0.0, 0.0</member>
191 </clone>
192
193 <clone prototype="skill_user_confirm">
194 <member id="title">Confirmation required</member>
195 <member id="text">Move T-piece to home pos?</member>
196 <member id="ignore" reference_id="ignore_uc" />
197 </clone>
198
199 <!-- Return to home pose while holding the T-piece -->
200 <clone prototype="skill_lin_time_optimal_ruckig">
201 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
202 <member id="target_frame">home_pos</member>
203 </clone>
204 </member>
205 </clone>
206
207 <!-- Skill sequence for inserting the T-piece in the guide with
208 champfers -->
209 <clone id="Insert T-piece (DfA conform guide)" prototype="skill_sequence">
210 <!-- Specify which robot will be controlled -->
211 <member id="robot" reference_id="environment.ur5"/>
212 <!-- The skills that will be carried out by the specified robot -->
213 <member id="skills">
214
215 <!-- Wait, until transformations are available -->
216 <clone prototype="skill_wait_for_transform">
217 <member id="frame">ur5_weiss_wsg50_grip_center</member>
218 <member id="parent_frame">world</member>
219 </clone>
220
221 <clone prototype="skill_user_confirm">
222 <member id="title">Confirmation required</member>
223 <member id="text">Move robot to home pos?</member>
224 <member id="ignore" reference_id="ignore_uc" />
225 </clone>
226
227 <!-- Move end effector to home position -->
228 <clone id="move robot to home pos" prototype="skill_lin_time_optimal_ruckig">
229 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
230 <member id="target_frame">home_pos</member>
231 </clone>
232
233 <clone prototype="skill_user_confirm">
234 <member id="title">Confirmation required</member>
235 <member id="text">Move robot to safe approach pos?</member>
236 <member id="ignore" reference_id="ignore_uc" />
237 </clone>
238
239 <!-- Move end effector to position closer to guide while
240 keeping a safe distance -->
241 <clone id="move to safe approach pos" prototype="skill_lin_time_optimal_ruckig">
242 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
243 <member id="target_frame">dfa_guide_safe_approach_pos</member>
244 </clone>
245
246 <clone prototype="skill_user_confirm">
247 <member id="title">Confirmation required</member>
248 <member id="text">Move robot to pre insertion pos?</member>
249 <member id="ignore" reference_id="ignore_uc" />
250 </clone>
251
252 <!-- Lower end effector with T-piece to the height of guide
253 opening -->
254 <clone id="move to close approach pos" prototype="skill_lin_time_optimal_ruckig">
255 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
256 <member id="target_frame">dfa_guide_close_approach_pos</member>
257 </clone>
258
259 <clone prototype="skill_user_confirm">
260 <member id="title">Confirmation required</member>
261 <member id="text">Move robot to pre contact pos?</member>
262 <member id="ignore" reference_id="ignore_uc" />
263 </clone>
264
265 <!-- Slowly move end effector with T-piece close to guide
266 opening, without physical contact between the workpiece and
267 the guide -->
268 <clone id="move end effector to pre contact position" prototype="skill_lin_time_optimal_ruckig">
269 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
270 <member id="target_frame">dfa_guide_pre_contact_pos</member>
271 <member id="max_linear_velocity">0.01</member>
272 </clone>
273
274 <clone prototype="skill_user_confirm">
275 <member id="title">Confirmation required</member>
276 <member id="text">Establish contact?</member>
277 <member id="ignore" reference_id="ignore_uc" />
278 </clone>
279
280 <!-- Tare force/torque sensor output -->
281 <clone id="tare ft sensor" prototype="skill_tare_gravity_compensator">
282 <member id="duration">0.1</member>
283 <member id="namespace">ur5</member>
284 </clone>
285
286 <!-- Move T-piece to guide opening until physical contact
287 has been established -->
288 <clone id="establish contact" prototype="skill_guarded_approach">
289 <member id="max_forces">5.0</member>
290 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
291 <member id="control_frame">table</member>
292 <member id="axes">y</member>
293 <member id="velocities">-0.005</member>
294 </clone>
295
296 <clone prototype="skill_user_confirm">
297 <member id="title">Confirmation required</member>
298 <member id="text">Slide with 15 N?</member>
299 <member id="ignore" reference_id="ignore_uc" />
300 </clone>
301
302 <!-- Slide the T-piece until it is completely inserted -->
303 <clone id="Guarded slide with 15 N" prototype="skill_slide">
304 <member id="control_frame">table</member>
305 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
306 <member id="move_axes">x, y, z, a, b, c</member>
307 <member id="velocities">0.0, -0.005, 0.0, 0.0, 0.0, 0.0</member>
308 <member id="force_axes">x, y, z, a, b, c</member>
309 <member id="target_forces">0.0, -12.5, 0.0, 0.0, 0.0, 0.0</member>
310 <member id="monitors">
311 <clone prototype="monitor_relative_distance">
312 <member id="event">succeeded</member>
313 <member id="reference_frame">table</member>
314 <member id="frame">ur5_weiss_wsg50_grip_center</member>
315 <member id="coordinates">y</member>
316 <member id="distance_coordinates">y</member>
317 <member id="distances">0.05</member>
318 <member id="operator">absolute_greater</member>
319 </clone>
320 </member>
321 </clone>
322
323 <!-- Slide the T-piece inside the guide until it touches the
324 limit of the guide with constant speed. The termination
325 criterion is the high forces that occur when the rear wall
326 of the guide is reached. -->
327 <clone id="Slide until guide end is reached" prototype="skill_guarded_slide">
328 <member id="control_frame">table</member>
329 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
330 <member id="max_forces">25.0</member>
331 <member id="operator">absolute_greater</member>
332 <member id="move_axes">y</member>
333 <member id="velocities">-0.005</member>
334 <member id="force_axes">x, y, z, a, b, c</member>
335 <member id="target_forces">0.0, -15.0, 0.0, 0.0, 0.0, 0.0</member>
336 </clone>
337
338 <clone prototype="skill_user_confirm">
339 <member id="title">Confirmation required</member>
340 <member id="text">Open gripper?</member>
341 <member id="ignore" reference_id="ignore_uc" />
342 </clone>
343
344 <!-- Open gripper to set position with set speed -->
345 <clone id="release T-piece" prototype="weiss_wsg50_release">
346 <member id="position">60.0</member>
347 <member id="speed">50.0</member>
348 </clone>
349
350 <clone prototype="skill_user_confirm">
351 <member id="title">Confirmation required</member>
352 <member id="text">Move robot upwards?</member>
353 <member id="ignore" reference_id="ignore_uc" />
354 </clone>
355
356 <!-- Because the position of the end effector is not fully
357 predictable when using force controlled motions, the gripper
358 is moved upwards relatively -->
359 <clone id="move end effector upwards" prototype="skill_relative_lin">
360 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
361 <member id="axes">x, y, z, a, b, c</member>
362 <member id="target_offsets">0.0, 0.0, -0.05, 0.0, 0.0, 0.0</member>
363 <member id="max_linear_velocity">0.05</member>
364 </clone>
365
366 <clone prototype="skill_user_confirm">
367 <member id="title">Confirmation required</member>
368 <member id="text">Move robot to home pos?</member>
369 <member id="ignore" reference_id="ignore_uc" />
370 </clone>
371
372 <!-- Move end effector back to home position -->
373 <clone id="move end effector to home pos" prototype="skill_lin_time_optimal_ruckig">
374 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
375 <member id="target_frame">home_pos</member>
376 </clone>
377 </member>
378 </clone>
379
380 <!-- Skill sequence for retrieving the T-piece from the guide with
381 champfers -->
382 <clone id="Retrieve T-piece (DfA conform guide)" prototype="skill_sequence">
383 <!-- Specify which robot will be controlled -->
384 <member id="robot" reference_id="environment.ur5"/>
385 <!-- The skills that will be carried out by the specified robot -->
386 <member id="skills">
387
388 <!-- Wait, until transformations are available -->
389 <clone prototype="skill_wait_for_transform">
390 <member id="frame">ur5_weiss_wsg50_grip_center</member>
391 <member id="parent_frame">world</member>
392 </clone>
393
394 <clone prototype="skill_user_confirm">
395 <member id="title">Confirmation required</member>
396 <member id="text">Move robot home?</member>
397 <member id="ignore" reference_id="ignore_uc" />
398 </clone>
399
400 <!-- Move end effector to home position -->
401 <clone id="" prototype="skill_lin_time_optimal_ruckig">
402 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
403 <member id="target_frame">home_pos</member>
404 </clone>
405
406 <!-- Make sure that gripper is opened -->
407 <clone id="Open gripper" prototype="weiss_wsg50_position">
408 <member id="position">60.0</member>
409 <member id="speed">50.0</member>
410 </clone>
411
412 <clone prototype="skill_user_confirm">
413 <member id="title">Confirmation required</member>
414 <member id="text">Move to position above T-piece?</member>
415 <member id="ignore" reference_id="ignore_uc" />
416 </clone>
417
418 <!-- Move end effector to position above gripping
419 position -->
420 <clone prototype="skill_lin_time_optimal_ruckig">
421 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
422 <member id="target_frame">dfa_guide_grasp_pos</member>
423 <member id="axes">x, y, z, a, b, c</member>
424 <member id="target_offsets">0.0, 0.0, -0.05, 0.0, 0.0, 0.0</member>
425 </clone>
426
427 <clone prototype="skill_user_confirm">
428 <member id="title">Confirmation required</member>
429 <member id="text">Move to grasp position?</member>
430 <member id="ignore" reference_id="ignore_uc" />
431 </clone>
432
433 <!-- Move end effector to gripping position -->
434 <clone prototype="skill_lin_time_optimal_ruckig">
435 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
436 <member id="target_frame">dfa_guide_grasp_pos</member>
437 <member id="axes">x, y, z, a, b, c</member>
438 </clone>
439
440 <!-- Tare the force/torque sensor -->
441 <clone prototype="skill_tare_gravity_compensator">
442 <member id="duration">0.1</member>
443 <member id="namespace">ur5</member>
444 </clone>
445
446 <clone prototype="skill_user_confirm">
447 <member id="title">Confirmation required</member>
448 <member id="text">Close gripper?</member>
449 <member id="ignore" reference_id="ignore_uc" />
450 </clone>
451
452 <!-- Close fingers to grip T-piece -->
453 <clone prototype="weiss_wsg50_grip">
454 <member id="position">30.0</member>
455 <member id="speed">50.0</member>
456 </clone>
457
458 <!-- Push down on T-piece to ensure safe grip -->
459 <clone id="ensure full grip on T-piece" prototype="skill_push_duration">
460 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
461 <member id="force_frame">ur5_weiss_wsg50_grip_center</member>
462 <member id="duration">1.0</member>
463 <member id="target_forces">0.0, 0.0, 15.0</member>
464 <member id="axes">x, y, z</member>
465 </clone>
466
467 <clone prototype="skill_user_confirm">
468 <member id="title">Confirmation required</member>
469 <member id="text">Slide T-piece out of guide?</member>
470 <member id="ignore" reference_id="ignore_uc" />
471 </clone>
472
473 <!-- Slide T-piece out of guide for a set distance -->
474 <clone id="Slide with 15 N" prototype="skill_slide">
475 <member id="control_frame">table</member>
476 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
477 <member id="move_axes">x, y, z, a, b, c</member>
478 <member id="velocities">0.0, 0.005, 0.0, 0.0, 0.0, 0.0</member>
479 <member id="force_axes">x, y, z, a, b, c</member>
480 <member id="target_forces">0.0, 12.5, 0.0, 0.0, 0.0, 0.0</member>
481 <member id="monitors">
482 <clone prototype="monitor_relative_distance">
483 <member id="event">succeeded</member>
484 <member id="reference_frame">table</member>
485 <member id="frame">ur5_weiss_wsg50_grip_center</member>
486 <member id="coordinates">y</member>
487 <member id="distance_coordinates">y</member>
488 <member id="distances">0.10</member>
489 <member id="operator">absolute_greater</member>
490 </clone>
491 </member>
492 </clone>
493
494 <clone prototype="skill_user_confirm">
495 <member id="title">Confirmation required</member>
496 <member id="text">Move end effector up?</member>
497 <member id="ignore" reference_id="ignore_uc" />
498 </clone>
499
500 <!-- Move the T-piece away from the guide to prevent
501 collision when returning to home pos -->
502 <clone prototype="skill_relative_lin">
503 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
504 <member id="axes">x, y, z, a, b, c</member>
505 <member id="target_offsets">0.05, 0.0, -0.05, 0.0, 0.0, 0.0</member>
506 <member id="max_linear_velocity">0.05</member>
507 </clone>
508
509 <clone prototype="skill_user_confirm">
510 <member id="title">Confirmation required</member>
511 <member id="text">Move robot to home pos?</member>
512 <member id="ignore" reference_id="ignore_uc" />
513 </clone>
514
515 <!-- Move end effector back to home pos -->
516 <clone prototype="skill_lin_time_optimal_ruckig">
517 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
518 <member id="target_frame">home_pos</member>
519 </clone>
520 </member>
521 </clone>
522
523 <!-- Skill sequence for inserting the T-piece in the guide without
524 champfers -->
525 <clone id="Insert T-piece (plain guide)" prototype="skill_sequence">
526 <!-- Specify which robot will be controlled -->
527 <member id="robot" reference_id="environment.ur5"/>
528 <!-- The skills that will be carried out by the specified robot -->
529 <member id="skills">
530
531 <!-- Wait, until transformations are available -->
532 <clone prototype="skill_wait_for_transform">
533 <member id="frame">ur5_weiss_wsg50_grip_center</member>
534 <member id="parent_frame">world</member>
535 </clone>
536
537 <clone prototype="skill_user_confirm">
538 <member id="title">Confirmation required</member>
539 <member id="text">Move robot to home pos?</member>
540 <member id="ignore" reference_id="ignore_uc" />
541 </clone>
542
543 <!-- Move end effector to home position -->
544 <clone id="move robot to home pos" prototype="skill_lin_time_optimal_ruckig">
545 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
546 <member id="target_frame">home_pos</member>
547 </clone>
548
549 <clone prototype="skill_user_confirm">
550 <member id="title">Confirmation required</member>
551 <member id="text">Move robot to safe approach pos?</member>
552 <member id="ignore" reference_id="ignore_uc" />
553 </clone>
554
555 <!-- Move end effector to safe position closer to guide
556 while holding T-piece -->
557 <clone id="move to safe approach pos" prototype="skill_lin_time_optimal_ruckig">
558 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
559 <member id="target_frame">plain_guide_safe_approach_pos</member>
560 </clone>
561
562 <clone prototype="skill_user_confirm">
563 <member id="title">Confirmation required</member>
564 <member id="text">Move robot to close approach pos?</member>
565 <member id="ignore" reference_id="ignore_uc" />
566 </clone>
567
568 <!-- Lower T-piece to guide height -->
569 <clone id="move to close approach pos" prototype="skill_lin_time_optimal_ruckig">
570 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
571 <member id="target_frame">plain_guide_close_approach_pos</member>
572 </clone>
573
574 <clone prototype="skill_user_confirm">
575 <member id="title">Confirmation required</member>
576 <member id="text">Move robot to pre contact pos?</member>
577 <member id="ignore" reference_id="ignore_uc" />
578 </clone>
579
580 <!-- Move T-piece closer to guide without establishing
581 contact -->
582 <clone id="move end effector to pre contact position" prototype="skill_lin_time_optimal_ruckig">
583 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
584 <member id="target_frame">plain_guide_pre_contact_pos</member>
585 <member id="max_linear_velocity">0.01</member>
586 </clone>
587
588 <!-- Tilt workpiece to extend lower left corner -->
589 <clone prototype="skill_relative_lin">
590 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
591 <member id="axes">x, y, z, a, b, c</member>
592 <member id="target_offsets">0.0, 0.0, -0.002, 0.0, -0.070, 0.070</member>
593 </clone>
594
595 <clone prototype="skill_user_confirm">
596 <member id="title">Confirmation required</member>
597 <member id="text">Establish contact?</member>
598 <member id="ignore" reference_id="ignore_uc" />
599 </clone>
600
601 <!-- Tare force/torque sensor output -->
602 <clone id="tare ft sensor" prototype="skill_tare_gravity_compensator">
603 <member id="duration">0.1</member>
604 <member id="namespace">ur5</member>
605 </clone>
606
607 <!-- Move T-piece closer to guide until contact is established -->
608 <clone id="establish contact" prototype="skill_guarded_approach">
609 <member id="max_forces">5.0</member>
610 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
611 <member id="control_frame">table</member>
612 <member id="axes">y</member>
613 <member id="velocities">0.005</member>
614 </clone>
615
616 <clone prototype="skill_user_confirm">
617 <member id="title">Confirmation required</member>
618 <member id="text">Push into bottom left corner?</member>
619 <member id="ignore" reference_id="ignore_uc" />
620 </clone>
621
622 <!-- Press bottom left corner of T-piece into bottom left
623 corner of guide -->
624 <clone id="push T-piece into bottom left corner of guide" prototype="skill_push_duration">
625 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
626 <member id="force_frame">table</member>
627 <member id="duration">2.0</member>
628 <member id="target_forces">-10.0, 10.0, -10.0</member>
629 <member id="axes">x, y, z</member>
630 </clone>
631
632 <clone prototype="skill_user_confirm">
633 <member id="title">Confirmation required</member>
634 <member id="text">Insert?</member>
635 <member id="ignore" reference_id="ignore_uc" />
636 </clone>
637
638 <!-- Insert by simultaneously aligning T-piece orientation
639 with guide and pressing into it -->
640 <clone prototype="skill_insert">
641 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
642 <member id="target_frame">plain_guide_pre_contact_pos</member>
643 <member id="force_axes">x, y, z</member>
644 <member id="track_axes">a,b,c</member>
645 <member id="track_offsets">0,0,0</member>
646 <member id="target_forces">-15.0, 0.0, 0.0</member>
647 <member id="monitors">
648 <clone prototype="monitor_duration">
649 <member id="event">succeeded</member>
650 <member id="duration">2.0</member>
651 </clone>
652 </member>
653 </clone>
654
655 <clone prototype="skill_user_confirm">
656 <member id="title">Confirmation required</member>
657 <member id="text">Slide with 15 N?</member>
658 <member id="ignore" reference_id="ignore_uc" />
659 </clone>
660
661 <!-- Make sure T-piece is further inserted by sliding it
662 into guide -->
663 <clone id="Slide with 15 N" prototype="skill_slide">
664 <member id="control_frame">table</member>
665 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
666 <member id="move_axes">x, y, z, a, b, c</member>
667 <member id="velocities">0.0, 0.005, 0.0, 0.0, 0.0, 0.0</member>
668 <member id="force_axes">x, y, z, a, b, c</member>
669 <member id="target_forces">0.0, 15.0, 3.0, 0.0, 0.0, 0.0</member>
670 <member id="monitors">
671 <clone prototype="monitor_duration">
672 <member id="event">succeeded</member>
673 <member id="duration">2.0</member>
674 </clone>
675 </member>
676 </clone>
677
678 <clone prototype="skill_user_confirm">
679 <member id="title">Confirmation required</member>
680 <member id="text">Press down on T-piece?</member>
681 <member id="ignore" reference_id="ignore_uc" />
682 </clone>
683
684 <!-- Ensure full grip by pressing down -->
685 <clone id="press down on T-piece" prototype="skill_push_duration">
686 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
687 <member id="force_frame">ur5_weiss_wsg50_grip_center</member>
688 <member id="duration">1.0</member>
689 <member id="target_forces">0.0, 0.0, 10.0</member>
690 <member id="axes">x, y, z</member>
691 </clone>
692
693 <clone prototype="skill_user_confirm">
694 <member id="title">Confirmation required</member>
695 <member id="text">Cycle through stress relief and slide?</member>
696 <member id="ignore" reference_id="ignore_uc" />
697 </clone>
698
699 <!-- Sub-sequence alternating between sliding and reducing
700 forces to prevent jamming of the T-piece -->
701 <clone prototype="skill_sequence">
702 <member id="robot" reference_id="environment.ur5"/>
703 <member id="skills">
704
705 <!-- Reduce forces -->
706 <clone id="reduce_forces" prototype="skill_apply_force">
707 <member id="force_frame">table</member>
708 <member id="axes">x, y, z</member>
709 <member id="target_forces">0.0, 0.0, 0.0</member>
710 <member id="monitors">
711 <clone prototype="monitor_duration">
712 <member id="event">succeeded</member>
713 <member id="duration">1.0</member>
714 </clone>
715 </member>
716 </clone>
717
718 <!-- Continue inserting the T-piece for specified
719 duration -->
720 <clone prototype="skill_insert">
721 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
722 <member id="target_frame">plain_guide_pre_contact_pos</member>
723 <member id="force_axes">x, z</member>
724 <member id="track_axes">a,b,c</member>
725 <member id="track_offsets">0,0,0</member>
726 <member id="target_forces">-25.0, 5.0</member>
727 <member id="transitions">
728 <clone prototype="transition">
729 <member id="event">succeeded</member>
730 <member id="target">reduce_forces</member>
731 </clone>
732 </member>
733 <member id="monitors">
734 <clone prototype="monitor_duration">
735 <member id="event">succeeded</member>
736 <member id="duration">1.0</member>
737 </clone>
738 </member>
739 </clone>
740 </member>
741
742 <!-- Monitor for the entire sub-sequence. Once the
743 requirement is met, the sequence will end -->
744 <member id="monitors">
745 <!-- Once the endeffector has reached a specified
746 absolute distance from when the sub-sequence
747 started, the sub-sequence will end -->
748 <clone prototype="monitor_relative_distance">
749 <member id="event">succeeded</member>
750 <member id="reference_frame">table</member>
751 <member id="frame">ur5_weiss_wsg50_grip_center</member>
752 <member id="coordinates">y</member>
753 <member id="distance_coordinates">y</member>
754 <member id="distances">0.07</member>
755 <member id="operator">absolute_greater</member>
756 </clone>
757 </member>
758 </clone>
759
760 <clone prototype="skill_user_confirm">
761 <member id="title">Confirmation required</member>
762 <member id="text">Insert for 10 seconds?</member>
763 <member id="ignore" reference_id="ignore_uc" />
764 </clone>
765
766 <!-- Ensure safe gripp before continuing to slide -->
767 <clone id="ensure full gripping contact" prototype="skill_push_duration">
768 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
769 <member id="force_frame">ur5_weiss_wsg50_grip_center</member>
770 <member id="duration">1.0</member>
771 <member id="target_forces">0.0, 0.0, 15.0</member>
772 <member id="axes">x, y, z</member>
773 </clone>
774
775 <!-- Slide for a set time so that T-piece reaches rear wall
776 of guide -->
777 <clone prototype="skill_insert">
778 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
779 <member id="target_frame">plain_guide_pre_contact_pos</member>
780 <member id="force_axes">x, y, z</member>
781 <member id="track_axes">a,b,c</member>
782 <member id="track_offsets">0,0,0</member>
783 <member id="target_forces">-25.0, 0.0, 3.0</member>
784 <member id="monitors">
785 <clone prototype="monitor_duration">
786 <member id="event">succeeded</member>
787 <member id="duration">3.0</member>
788 </clone>
789 </member>
790 </clone>
791
792 <clone prototype="skill_user_confirm">
793 <member id="title">Confirmation required</member>
794 <member id="text">Open gripper?</member>
795 <member id="ignore" reference_id="ignore_uc" />
796 </clone>
797
798 <!-- Open gripper until specified position is reached -->
799 <clone id="release T-piece" prototype="weiss_wsg50_release">
800 <member id="position">60.0</member>
801 <member id="speed">50.0</member>
802 </clone>
803
804 <clone prototype="skill_user_confirm">
805 <member id="title">Confirmation required</member>
806 <member id="text">Move robot to home pos?</member>
807 <member id="ignore" reference_id="ignore_uc" />
808 </clone>
809
810 <!-- Move robot back to home position -->
811 <clone id="return home" prototype="skill_lin_time_optimal_ruckig">
812 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
813 <member id="target_frame">home_pos</member>
814 </clone>
815 </member>
816 </clone>
817
818 <!-- Skill sequence for retrieving the T-piece from the guide with
819 champfers -->
820 <clone id="Retrieve T-piece (plain guide)" prototype="skill_sequence">
821 <!-- Specify which robot will be controlled -->
822 <member id="robot" reference_id="environment.ur5"/>
823 <!-- The skills that will be carried out by the specified robot -->
824 <member id="skills">
825
826 <!-- Wait, until transformations are available -->
827 <clone prototype="skill_wait_for_transform">
828 <member id="frame">ur5_weiss_wsg50_grip_center</member>
829 <member id="parent_frame">world</member>
830 </clone>
831
832 <clone prototype="skill_user_confirm">
833 <member id="title">Confirmation required</member>
834 <member id="text">Move robot home?</member>
835 <member id="ignore" reference_id="ignore_uc" />
836 </clone>
837
838 <!-- Move end effector to home pos -->
839 <clone id="move to home pos" prototype="skill_lin_time_optimal_ruckig">
840 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
841 <member id="target_frame">home_pos</member>
842 </clone>
843
844 <!-- Make sure gripper is open -->
845 <clone id="open gripper" prototype="weiss_wsg50_position">
846 <member id="position">60.0</member>
847 <member id="speed">50.0</member>
848 </clone>
849
850 <clone prototype="skill_user_confirm">
851 <member id="title">Confirmation required</member>
852 <member id="text">Move to position above T-piece?</member>
853 <member id="ignore" reference_id="ignore_uc" />
854 </clone>
855
856 <!-- Move end effector to position above T-piece -->
857 <clone prototype="skill_lin_time_optimal_ruckig">
858 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
859 <member id="target_frame">plain_guide_grasp_pos</member>
860 <member id="axes">x, y, z, a, b, c</member>
861 <member id="target_offsets">0.0, 0.0, -0.05, 0.0, 0.0, 0.0</member>
862 </clone>
863
864 <clone prototype="skill_user_confirm">
865 <member id="title">Confirmation required</member>
866 <member id="text">Move to grasp position?</member>
867 <member id="ignore" reference_id="ignore_uc" />
868 </clone>
869
870 <!-- Move end effector to gripping position -->
871 <clone prototype="skill_lin_time_optimal_ruckig">
872 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
873 <member id="target_frame">plain_guide_grasp_pos</member>
874 <member id="axes">x, y, z, a, b, c</member>
875 </clone>
876
877 <!-- Tare force/torque sensor output -->
878 <clone prototype="skill_tare_gravity_compensator">
879 <member id="duration">0.1</member>
880 <member id="namespace">ur5</member>
881 </clone>
882
883 <clone prototype="skill_user_confirm">
884 <member id="title">Confirmation required</member>
885 <member id="text">Close gripper?</member>
886 <member id="ignore" reference_id="ignore_uc" />
887 </clone>
888
889 <!-- Gripp T-piece -->
890 <clone prototype="weiss_wsg50_grip">
891 <member id="position">30.0</member>
892 <member id="speed">50.0</member>
893 </clone>
894
895 <!-- Push down on T-piece to ensure full grip -->
896 <clone id="ensure full grip on T-piece" prototype="skill_push_duration">
897 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
898 <member id="force_frame">ur5_weiss_wsg50_grip_center</member>
899 <member id="duration">1.0</member>
900 <member id="target_forces">0.0, 0.0, 15.0</member>
901 <member id="axes">x, y, z</member>
902 </clone>
903
904 <clone prototype="skill_user_confirm">
905 <member id="title">Confirmation required</member>
906 <member id="text">Slide T-piece out of guide?</member>
907 <member id="ignore" reference_id="ignore_uc" />
908 </clone>
909
910 <!-- Slide T-piece completely out of guide -->
911 <clone id="Slide with 15 N" prototype="skill_slide">
912 <member id="control_frame">table</member>
913 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
914 <member id="move_axes">x, y, z, a, b, c</member>
915 <member id="velocities">0.0, -0.005, 0.0, 0.0, 0.0, 0.0</member>
916 <member id="force_axes">x, y, z, a, b, c</member>
917 <member id="target_forces">0.0, 12.5, 0.0, 0.0, 0.0, 0.0</member>
918 <member id="monitors">
919 <clone prototype="monitor_relative_distance">
920 <member id="event">succeeded</member>
921 <member id="reference_frame">table</member>
922 <member id="frame">ur5_weiss_wsg50_grip_center</member>
923 <member id="coordinates">y</member>
924 <member id="distance_coordinates">y</member>
925 <member id="distances">0.10</member>
926 <member id="operator">absolute_greater</member>
927 </clone>
928 </member>
929 </clone>
930
931 <clone prototype="skill_user_confirm">
932 <member id="title">Confirmation required</member>
933 <member id="text">Move end effector up?</member>
934 <member id="ignore" reference_id="ignore_uc" />
935 </clone>
936
937 <!-- Move T-piece away from guide and upwards -->
938 <clone prototype="skill_relative_lin">
939 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
940 <member id="axes">x, y, z, a, b, c</member>
941 <member id="target_offsets">0.05, 0.0, -0.05, 0.0, 0.0, 0.0</member>
942 <member id="max_linear_velocity">0.05</member>
943 </clone>
944
945 <clone prototype="skill_user_confirm">
946 <member id="title">Confirmation required</member>
947 <member id="text">Move robot to home pos?</member>
948 <member id="ignore" reference_id="ignore_uc" />
949 </clone>
950
951 <!-- Move end effector back to home pos -->
952 <clone prototype="skill_lin_time_optimal_ruckig">
953 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
954 <member id="target_frame">home_pos</member>
955 </clone>
956 </member>
957 </clone>
958
959 <!-- Skill sequence for placing the T-piece in workpiece holder -->
960 <clone id="Place T-piece in workpiece holder" prototype="skill_sequence">
961 <!-- Specify which robot will be controlled -->
962 <member id="robot" reference_id="environment.ur5"/>
963 <!-- The skills that will be carried out by the specified robot -->
964 <member id="skills">
965
966 <!-- Wait, until transformations are available -->
967 <clone prototype="skill_wait_for_transform">
968 <member id="frame">ur5_weiss_wsg50_grip_center</member>
969 <member id="parent_frame">world</member>
970 </clone>
971
972 <clone prototype="skill_user_confirm">
973 <member id="title">Confirmation required</member>
974 <member id="text">Move robot to home pos?</member>
975 <member id="ignore" reference_id="ignore_uc" />
976 </clone>
977
978 <!-- Move end effector to home position -->
979 <clone prototype="skill_lin_time_optimal_ruckig">
980 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
981 <member id="target_frame">home_pos</member>
982 </clone>
983
984 <!-- Tare force/torque sensor output -->
985 <clone prototype="skill_tare_gravity_compensator">
986 <member id="duration">0.1</member>
987 <member id="namespace">ur5</member>
988 </clone>
989
990 <clone prototype="skill_user_confirm">
991 <member id="title">Confirmation required</member>
992 <member id="text">Move robot to safe approach pos above T-piece?</member>
993 <member id="ignore" reference_id="ignore_uc" />
994 </clone>
995
996 <!-- Move end effector holding the T-piece to safe position above
997 workpiece holder -->
998 <clone prototype="skill_lin_time_optimal_ruckig">
999 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
1000 <member id="target_frame">t_piece_safe_approach_pos</member>
1001 </clone>
1002
1003 <clone prototype="skill_user_confirm">
1004 <member id="title">Confirmation required</member>
1005 <member id="text">Move robot to pre placing position?</member>
1006 <member id="ignore" reference_id="ignore_uc" />
1007 </clone>
1008
1009 <!-- Move end effector close to workiece holder without
1010 establishing contact between T-piece and workpiece
1011 holder -->
1012 <clone prototype="skill_lin_time_optimal_ruckig">
1013 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
1014 <member id="target_frame">t_piece_grasp_pos</member>
1015 <member id="axes">x, y, z, a, b, c</member>
1016 <member id="target_offsets">0.0, 0.0, -0.01, 0.0, 0.0, 0.0</member>
1017 </clone>
1018
1019 <clone prototype="skill_user_confirm">
1020 <member id="title">Confirmation required</member>
1021 <member id="text">Place down T-piece?</member>
1022 <member id="ignore" reference_id="ignore_uc" />
1023 </clone>
1024
1025 <!-- Move T-piece in direction of workpiece holder until
1026 defined force is reached to establish contact -->
1027 <clone id="establish contact" prototype="skill_guarded_approach">
1028 <member id="max_forces">5.0</member>
1029 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
1030 <member id="control_frame">ur5_weiss_wsg50_grip_center</member>
1031 <member id="axes">z</member>
1032 <member id="velocities">0.005</member>
1033 </clone>
1034
1035 <!-- Push down on T-piece to make sure it is inserted into
1036 recess in workpiece holder -->
1037 <clone id="place T-piece" prototype="skill_push_duration">
1038 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
1039 <member id="force_frame">ur5_weiss_wsg50_grip_center</member>
1040 <member id="duration">1.0</member>
1041 <member id="target_forces">0.0, 0.0, 5.0</member>
1042 <member id="axes">x, y, z</member>
1043 </clone>
1044
1045 <clone prototype="skill_user_confirm">
1046 <member id="title">Confirmation required</member>
1047 <member id="text">Open gripper?</member>
1048 <member id="ignore" reference_id="ignore_uc" />
1049 </clone>
1050
1051 <!-- Open gripper to set position with set speed -->
1052 <clone id="release T-piece" prototype="weiss_wsg50_release">
1053 <member id="position">60.0</member>
1054 <member id="speed">50.0</member>
1055 </clone>
1056
1057 <clone prototype="skill_user_confirm">
1058 <member id="title">Confirmation required</member>
1059 <member id="text">Move robot upwards?</member>
1060 <member id="ignore" reference_id="ignore_uc" />
1061 </clone>
1062
1063 <!-- Move end effector upwards -->
1064 <clone id="move end effector_upwards" prototype="skill_lin_time_optimal_ruckig">
1065 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
1066 <member id="target_frame">t_piece_grasp_pos</member>
1067 <member id="axes">x, y, z, a, b, c</member>
1068 <member id="target_offsets">0.0, 0.0, -0.05, 0.0, 0.0, 0.0</member>
1069 </clone>
1070
1071 <clone prototype="skill_user_confirm">
1072 <member id="title">Confirmation required</member>
1073 <member id="text">Move robot to home pos?</member>
1074 <member id="ignore" reference_id="ignore_uc" />
1075 </clone>
1076
1077 <!-- Move end effector back to home position -->
1078 <clone id="return to home pos" prototype="skill_lin_time_optimal_ruckig">
1079 <member id="tool_frame">ur5_weiss_wsg50_grip_center</member>
1080 <member id="target_frame">home_pos</member>
1081 </clone>
1082 </member>
1083 </clone>
1084 </member>
1085 </clone>
1086</pitasc>