2525# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
2626# POSSIBILITY OF SUCH DAMAGE.
2727
28- #!/usr/bin/env python
29- # -*- coding: utf-8 -*-
30-
31- # @package libherb.tsr Utilities for TSRs and TSR chains.
32-
3328import openravepy
3429import numpy
3530import numpy .random
3631import kin
3732
38- """
39- Functions for Serializing TSRs and TSR Chains
40-
41- SerializeTSR(manipindex,bodyandlink,T0_w,Tw_e,Bw)
42-
43- Input:
44- manipindex (int): the 0-indexed index of the robot's manipulator
45- bodyandlink (str): body and link which is used as the 0 frame. Format
46- 'body_name link_name'. To use world frame, specify 'NULL'
47- T0_w (double 4x4): transform matrix of the TSR's reference frame relative to
48- the 0 frame
49- Tw_e (double 4x4): transform matrix of the TSR's offset frame relative to the
50- w frame
51- Bw (double 1x12): bounds in x y z roll pitch yaw.
52- Format: [x_min, x_max, y_min, y_max ...]
53-
54- Output:
55- outstring (str): string to use for SerializeTSRChain function
56-
57- SerializeTSRChain(bSampleFromChain,
58- bConstrainToChain,
59- numTSRs,
60- allTSRstring,
61- mimicbodyname,
62- mimicbodyjoints)
63-
64- Input:
65- bSampleStartFromChain (0/1): 1: Use this chain for sampling start configs
66- 0: Ignore for sampling starts
67- bSampleGoalFromChain (0/1): 1: Use this chain for sampling goal configs
68- 0: Ignore for sampling goals
69- bConstrainToChain (0/1): 1: Use this chain for constraining configs
70- 0: Ignore for constraining
71- numTSRs (int): Number of TSRs in this chain (must be > 0)
72- allTSRstring (str): string of concetenated TSRs generated using SerializeTSR.
73- Should be like [TSRstring 1 ' ' TSRstring2 ...]
74- mimicbodyname (str): name of associated mimicbody for this chain
75- (NULL if none associated)
76- mimicbodyjoints (int [1xn]): 0-indexed indices of the mimicbody's joints that
77- are mimiced (MUST BE INCREASING AND CONSECUTIVE)
78-
79- Output:
80- outstring (str): string to include in call to cbirrt planner
81- """
82-
83-
84- def SerializeTransform12Col (tm , format = '%.5f' ):
85- return ' ' .join ([(format % (i ,)) for i in tm [0 :3 , :].T .reshape (12 )])
86-
87-
88- def SerializeArray (a , format = '%.5f' ):
89- return ' ' .join ([(format % (i ,)) for i in a .reshape (- 1 )])
90-
91-
92- class TSR (object ): # force new-style class
9333
34+ class TSR (object ):
35+ """ A Task-Space-Region (TSR) represents a motion constraint. """
9436 def __init__ (self , T0_w = None , Tw_e = None , Bw = None ,
9537 manip = None , bodyandlink = 'NULL' ):
9638 if T0_w is None :
@@ -140,13 +82,8 @@ def sample(self, vals=None):
14082 trans = numpy .dot (numpy .dot (self .T0_w , Tw ), self .Tw_e )
14183 return trans
14284
143- def serialize (self ):
144- return '%d %s %s %s %s' % (self .manipindex , self .bodyandlink ,
145- SerializeTransform12Col (self .T0_w ),
146- SerializeTransform12Col (self .Tw_e ),
147- SerializeArray (self .Bw ))
148-
149- def serialize_dict (self ):
85+ def to_dict (self ):
86+ """ Convert this TSR to a python dict. """
15087 return {
15188 'T0_w' : self .T0_w .tolist (),
15289 'Tw_e' : self .Tw_e .tolist (),
@@ -156,15 +93,49 @@ def serialize_dict(self):
15693 }
15794
15895 @staticmethod
159- def deserialize_dict (x ):
96+ def from_dict (x ):
97+ """ Construct a TSR from a python dict. """
16098 return TSR (
161- T0_w = numpy .array (x ['T0_w' ]),
162- Tw_e = numpy .array (x ['Tw_e' ]),
163- Bw = numpy .array (x ['Bw' ]),
164- manip = numpy .array (x [ 'manipindex' ] ),
165- bodyandlink = numpy .array (x [ 'bodyandlink' ] )
99+ T0_w = numpy .array (x ['T0_w' ]),
100+ Tw_e = numpy .array (x ['Tw_e' ]),
101+ Bw = numpy .array (x ['Bw' ]),
102+ manip = numpy .array (x . get ( 'manipindex' , - 1 ) ),
103+ bodyandlink = numpy .array (x . get ( 'bodyandlink' , 'NULL' ) )
166104 )
167105
106+ def to_json (self ):
107+ """ Convert this TSR to a JSON string. """
108+ import json
109+ return json .dumps (self .to_dict ())
110+
111+ @staticmethod
112+ def from_json (x , * args , ** kw_args ):
113+ """
114+ Construct a TSR from a JSON string.
115+
116+ This method internally forwards all arguments to `json.loads`.
117+ """
118+ import json
119+ x_dict = json .loads (x , * args , ** kw_args )
120+ return TSR .from_dict (x_dict )
121+
122+ def to_yaml (self ):
123+ """ Convert this TSR to a YAML string. """
124+ import yaml
125+ return yaml .dumps (self .to_dict ())
126+
127+ @staticmethod
128+ def from_yaml (x , * args , ** kw_args ):
129+ """
130+ Construct a TSR from a YAML string.
131+
132+ This method internally forwards all arguments to `yaml.safe_load`.
133+ """
134+ import yaml
135+ x_dict = yaml .safe_load (x , * args , ** kw_args )
136+ return TSR .from_dict (x_dict )
137+
138+
168139class TSRChain (object ):
169140
170141 def __init__ (self , sample_start = False , sample_goal = False , constrain = False ,
@@ -205,41 +176,61 @@ def __init__(self, sample_start=False, sample_goal=False, constrain=False,
205176 def append (self , tsr ):
206177 self .TSRs .append (tsr )
207178
208- def serialize (self ):
209- allTSRstring = ' ' .join ([tsr .serialize () for tsr in self .TSRs ])
210- numTSRs = len (self .TSRs )
211- outstring = '%d %d %d' % (int (self .sample_start ),
212- int (self .sample_goal ),
213- int (self .constrain ))
214- outstring += ' %d %s' % (numTSRs , allTSRstring )
215- outstring += ' ' + self .mimicbodyname
216- if len (self .mimicbodyjoints ) > 0 :
217- outstring += ' %d %s' % (len (self .mimicbodyjoints ),
218- SerializeArray (self .mimicbodyjoints ))
219- return outstring
220-
221- def serialize_dict (self ):
179+ def to_dict (self ):
180+ """ Construct a TSR chain from a python dict. """
222181 return {
223182 'sample_goal' : self .sample_goal ,
224183 'sample_start' : self .sample_start ,
225184 'constrain' : self .constrain ,
226185 'mimicbodyname' : self .mimicbodyname ,
227186 'mimicbodyjoints' : self .mimicbodyjoints ,
228- 'tsrs' : [ tsr .serialize_dict () for tsr in self .TSRs ],
187+ 'tsrs' : [tsr .to_dict () for tsr in self .TSRs ],
229188 }
230189
231-
232190 @staticmethod
233- def deserialize_dict (x ):
191+ def from_dict (x ):
192+ """ Construct a TSR chain from a python dict. """
234193 return TSRChain (
235194 sample_start = x ['sample_start' ],
236195 sample_goal = x ['sample_goal' ],
237196 constrain = x ['constrain' ],
238- TSRs = [ TSR .deserialize_dict (tsr ) for tsr in x ['tsrs' ] ],
197+ TSRs = [TSR .from_dict (tsr ) for tsr in x ['tsrs' ]],
239198 mimicbodyname = x ['mimicbodyname' ],
240199 mimicbodyjoints = x ['mimicbodyjoints' ],
241200 )
242201
202+ def to_json (self ):
203+ """ Convert this TSR chain to a JSON string. """
204+ import json
205+ return json .dumps (self .to_dict ())
206+
207+ @staticmethod
208+ def from_json (x , * args , ** kw_args ):
209+ """
210+ Construct a TSR chain from a JSON string.
211+
212+ This method internally forwards all arguments to `json.loads`.
213+ """
214+ import json
215+ x_dict = json .loads (x , * args , ** kw_args )
216+ return TSR .from_dict (x_dict )
217+
218+ def to_yaml (self ):
219+ """ Convert this TSR chain to a YAML string. """
220+ import yaml
221+ return yaml .dumps (self .to_dict ())
222+
223+ @staticmethod
224+ def from_yaml (x , * args , ** kw_args ):
225+ """
226+ Construct a TSR chain from a YAML string.
227+
228+ This method internally forwards all arguments to `yaml.safe_load`.
229+ """
230+ import yaml
231+ x_dict = yaml .safe_load (x , * args , ** kw_args )
232+ return TSR .from_dict (x_dict )
233+
243234 def sample (self ):
244235 if len (self .TSRs ) == 0 :
245236 return None
0 commit comments