You signed in with another tab or window. Reload to refresh your session.You signed out in another tab or window. Reload to refresh your session.You switched accounts on another tab or window. Reload to refresh your session.Dismiss alert
Copy file name to clipboardExpand all lines: src/prpy/tsr/README.md
+30-27Lines changed: 30 additions & 27 deletions
Original file line number
Diff line number
Diff line change
@@ -1,19 +1,21 @@
1
1
# Task Space Regions
2
2
3
-
This directory contains the python interfaces necessary to specify Task Space Regions (TSRs). For a detailed description of TSRs and their uses, please refer to the 2010 IJRR paper entitled "Task Space Regions: A Framework for Pose-Constrained
3
+
This directory contains the Python interfaces necessary to specify Task Space Regions (TSRs). For a detailed description of TSRs and their uses, please refer to the 2010 IJRR paper entitled "Task Space Regions: A Framework for Pose-Constrained
4
4
Manipulation Planning" by Dmitry Berenson, Siddhartha Srinivasa, and James Kuffner. A copy of this publication can be downloaded [here](https://www.ri.cmu.edu/pub_files/2011/10/dmitry_ijrr10-1.pdf).
5
5
6
6
## TSR Overview
7
-
A TSR is typically used to defined a constraint on the pose of the end-effector of a manipulator. For example consider a manipulator tasked with grabbing a glass. The end-effector (hand) must be near the glass, and oriented in a way that allows the fingers to grab around the glass when closed. This set of workspace constraints on valid poses of the end-effector can be expressed as a TSR.
7
+
A TSR is typically used to defined a constraint on the pose of the end-effector of a manipulator. For example, consider a manipulator tasked with grabbing a glass. The end-effector (hand) must be near the glass, and oriented in a way that allows the fingers to grab around the glass when closed. This set of workspace constraints on valid poses of the end-effector can be expressed as a TSR.
8
8
9
9
A TSR is defined by three components:
10
-
*```T0_w``` - A transform from the world origin to the TSR frame w
11
-
*```Tw_e``` - A transform from the TSR frame w to the end-effector
12
-
*```Bw``` - A 6x2 matrix of bounds in the coordinates of w
10
+
*`T0_w` - A transform from the world frame to the TSR frame w
11
+
*`Tw_e` - A transform from the TSR frame w to the end-effector
12
+
*`Bw` - A 6x2 matrix of bounds on the coordinates of w
13
13
14
-
The first three rows of Bw bound the allowable translation along the x,y and z axes (in meters). The last three rows bound the allowable rotation about those axes (in radians), all in w frame. Note that this asumed Roll-Pitch-Yaw (RPY) Euler angle convention.
14
+
The first three rows of `Bw` bound the allowable translation along the x,y and z axes (in meters). The last three rows bound the allowable rotation about those axes in w frame. The rotation is expressed using the Roll-Pitch-Yaw (RPY) Euler angle convention and has units of radians.
15
15
16
-
Note that the end-effector frame is a robot-specific frame. In OpenRAVE, you can obtain the pose of the end-effector using ```GetEndEffectorTransform()``` on the manipulator. The following code snippet visualizes the end-effector frame of the robot's right arm:
16
+
Note that the end-effector frame is a robot-specific frame. In OpenRAVE, you can obtain the pose of the end-effector using ```GetEndEffectorTransform()``` on the manipulator. This *is not* equivilent to calling `GetTransform()` on the end-effector frame because these transformations differ by `GetLocalToolTransform()`.
17
+
18
+
The following code snippet visualizes the end-effector frame of the robot's right arm:
17
19
```python
18
20
ipython>import openravepy
19
21
ipython> h = openravepy.misc.DrawAxes(env, robot.right_arm.GetEndEffectorTransform())
A single TSR, or finite set of TSRs, is sometimes insufficient to capture pose constraints of a given task. To describe more complex constraints, such as closed-chain kinematics, we can use a TSR Chain. Consider the example of opening a refrigerator door while allowing the manipulator to rotate around the handle. Here, the constraint on the motion of the hand is defined by the product of two constraints. The first constraint describes valid locations of the handle, which all lie on the arc defined by the position of the handle relative to the door hinge. The second constraint defines the position of the robot end-effector relative to the handle. Each of these constraints can be defined by a single TSR. In order to specify the full constraint on the hand motion, we link the TSRs in a TSR Chain.
59
+
A single TSR, or finite set of TSRs, is sometimes insufficient to capture pose constraints of a given task. To describe more complex constraints, such as closed-chain kinematics, we can use a TSR Chain. Consider the example of opening a refrigerator door while allowing the manipulator to rotate around the handle. Here, the constraint on the motion of the hand is defined by the composition of two constraints. The first constraint describes valid locations of the handle, which all lie on the arc defined by the position of the handle relative to the door hinge. The second constraint defines the position of the robot end-effector relative to the handle. Each of these constraints can be defined by a single TSR. In order to specify the full constraint on the hand motion, we link the TSRs in a TSR Chain.
58
60
59
61
### Example: Defining a TSR Chain
60
62
In the following code snippet, we show how to define a TSR Chain for the example of opening the refrigerator door, allowing the robot's hand to rotate around the door handle.
61
63
62
-
First we define the TSR that constrains the pose of the handle
64
+
First we define the TSR that constrains the pose of the handle:
63
65
```python
64
66
ipython> T0_w = hinge_pose # hinge_pose is a 4x4 matrix defining the pose of the hinge in world frame
65
67
# Now define Tw_e as the pose of the handle relative to the hinge
Similar to the TSRs, we can sample and compute distance to chains using the ```sample``` and ```distance``` functions respectively. The ```sample_start```, ```sample_goal``` and ```constrain``` flags will be explained in the next section.
92
94
93
-
## Prpy Planning support for TSRs
94
-
Several of the planners in the prpy [planning pipeline](https://github.com/personalrobotics/prpy/tree/master/src/prpy/planning) have some support for using TSRs for defining constriants through the ```PlanToTSR```method. The method accepts as a list of ```TSRChain``` objects. The ```sample_start```, ```sample_goal``` and ```constrain``` flags on the each ```TSRChain``` indicate to the planner how the chain should be used.
95
+
## Planning with TSRs
96
+
Several of the planners in the prpy [planning pipeline](https://github.com/personalrobotics/prpy/tree/master/src/prpy/planning) have support for using TSRs for defining start sets, goal sets, and trajectory-wide constraints through the `PlanToTSR` planning method. The method accepts as a list of `TSRChain` objects. The `sample_start`, `sample_goal` and `constrain` flags on the each `TSRChain` indicate to the planner how the chain should be used.
95
97
96
-
### Example: Planning to a single TSR
97
-
Consider the example of grasping a glass. Given our ```grasp_tsr``` we would now like to generate a plan that moves the robot to any configuration such that the end-effector meets the constraint defined by the tsr. The following code can be used to do this:
98
+
### Example: Planning to a TSR goal
99
+
Consider the example of grasping a glass. Given our `grasp_tsr` we would now like to generate a plan that moves the robot to any configuration such that the end-effector meets the constraint defined by the tsr. The following code can be used to do this:
Defining ```sample_goal=True``` tells the planner to apply the constraint only to the last point in the plan. Now we can call the planner:
104
+
Defining `sample_goal=True` tells the planner to apply the constraint only to the last point in the plan. Now we can call the planner:
103
105
```python
104
106
ipython> traj = robot.PlanToTSR([tsrchain])
105
107
```
106
-
### Example: Planning from a single TSR
107
-
Now imagine we wish to generate a plan that starts from any point in the grasp TSR and plans to a defined configuration, ```config```. The following code can be used to do this:
108
+
### Example: Planning from a TSR start
109
+
Now imagine we wish to generate a plan that starts from any point in the grasp TSR and plans to a defined configuration, `config`. The following code can be used to do this:
### Example: Apply a TSR constraint across a full trajectory
117
-
In the refrigerator opening example, the TSR Chain defined a constraint on the motion of the end-effector that should be applied over the whole trajectory. We defined:
118
+
### Example: Planning with a trajectory-wide TSR constraint
119
+
In the refrigerator opening example, the TSR chain defined a constraint on the motion of the end-effector that should be applied over the whole trajectory. We defined:
Here, the caller must be careful to ensure that ```config``` meets the constraint defined by the TSR.
127
129
128
-
### Example: Planning to a set of TSRs
129
-
Now imagine we had to TSRs, ```grasp1_tsr``` and ```grasp2_tsr``` the each defined a set of valid configurations for grasping. We can ask the planner to generate a plan to any configuration that meets either the ```grasp1_tsr``` or the ```grasp2_tsr``` constraint in the following way:
130
+
### Example: Planning to multiple TSR goals
131
+
Now imagine we had to TSRs, `grasp1_tsr` and `grasp2_tsr` the each defined a set of valid configurations for grasping. We can ask the planner to generate a plan to any configuration that meets either the `grasp1_tsr` or the `grasp2_tsr` constraint in the following way:
0 commit comments