Skip to content

Commit 4cd2158

Browse files
committed
Update README.md
1 parent dad7bc8 commit 4cd2158

File tree

1 file changed

+37
-0
lines changed

1 file changed

+37
-0
lines changed

src/prpy/tsr/README.md

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -49,3 +49,40 @@ ipython> meets_constraint = (dist_to_tsr == 0.0)
4949
```
5050

5151
## TSR Chains
52+
A single TSR, or finite set of TSRs, is sometimes insufficient to capture pose ocnstraints 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.
53+
54+
### Example: Defining a TSR Chain
55+
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.
56+
57+
First we define the TSR that constrains the pose of the handle
58+
```python
59+
ipython> T0_w = hinge_pose # hinge_pose is a 4x4 matrix defining the pose of the hinge in world frame
60+
# Now define Tw_e as the pose of the handle relative to the hinge
61+
ipython> Tw_e = numpy.eye() # Same orientation as the hinge frame
62+
ipython> Tw_e[0,3] = 0.4 # The handle is offset 40cm from the hinge along the x-axis of the hinge-frame
63+
ipython> Bw = numpy.zeros((6,2)) # Assume the handle is fixed
64+
ipython> fridge = env.GetKinBody('refridgerator')
65+
ipython> fridge.SetActiveManipulator('door')
66+
ipython> door_idx = fridge.GetActiveManipulatorIndex()
67+
ipython> constraint1 = prpy.tsr.TSR(T0_w = T0_w, Tw_e = Tw_e, Bw = Bw, manip = door_idx)
68+
```
69+
70+
Next we define the TSR that constraints the pose of the hand relative to the handle
71+
```python
72+
ipython> T0_w = numpy.eye(4) # This will be ignored once we compose the chain
73+
ipython> Tw_e = ee_in_handle # A 4x4 defining the desire pose of the end-effector relative to handle
74+
ipython> Bw = numpy.zeros((6,2))
75+
ipython> Bw(5,:) = [-0.25*numpy.pi, 0.25*numpy.pi]
76+
ipython> robot.right_arm.SetActive() # use the right arm to grab the door
77+
ipython> manip_idx = robot.GetActiveManipulatorIndex()
78+
ipython> constraint2 = prpy.tsr.TSR(T0_w = T0_w, Tw_e = Tw_e, Bw = Bw, manip = manip_idx)
79+
```
80+
81+
Finally, we compose these into a chain
82+
```python
83+
ipython> tsrchain = prpy.tsr.TSRChain(sample_start=False, sample_goal=False, constrain=True,
84+
TSRs = [constraint1, constraint2])
85+
```
86+
Similar to the TSRs, we can sample and compute distance to chains using the ```sample``` and ```distance``` functions respectively.
87+
88+
## Prpy Planning support for TSRs

0 commit comments

Comments
 (0)