@@ -80,6 +80,7 @@ def __init__(
8080 tol : float = 1e-6 ,
8181 mask : Union [ArrayLike , None ] = None ,
8282 joint_limits : bool = True ,
83+ seed : Union [int , None ] = None ,
8384 ):
8485 """
8586 name: The name of the IK algorithm
@@ -102,6 +103,9 @@ def __init__(
102103 self .ilimit = ilimit
103104 self .tol = tol
104105
106+ # Random number generator
107+ self ._private_random = np .random .default_rng (seed = seed )
108+
105109 if mask is None :
106110 mask = np .ones (6 )
107111
@@ -143,7 +147,6 @@ def solve(
143147 # Initialise variables
144148 E = 0.0
145149 q = q0 [0 ]
146- jl_valid = False
147150
148151 for search in range (self .slimit ):
149152 q = q0 [search ].copy ()
@@ -254,6 +257,33 @@ def step(
254257 """
255258 pass
256259
260+ def random_q (self , ets : "rtb.ETS" , i : int = 1 ) -> np .ndarray :
261+ """
262+ Generate a random valid joint configuration
263+
264+ :param i: number of configurations to generate
265+
266+ Generates a random q vector within the joint limits defined by
267+ `self.qlim`.
268+ """
269+
270+ if i == 1 :
271+ q = np .zeros (ets .n )
272+
273+ for i in range (ets .n ):
274+ q [i ] = self ._private_random .uniform (ets .qlim [0 , i ], ets .qlim [1 , i ])
275+
276+ else :
277+ q = np .zeros ((i , ets .n ))
278+
279+ for j in range (i ):
280+ for i in range (ets .n ):
281+ q [j , i ] = self ._private_random .uniform (
282+ ets .qlim [0 , i ], ets .qlim [1 , i ]
283+ )
284+
285+ return q
286+
257287
258288def null_Σ (ets : "rtb.ETS" , q : np .ndarray , ps : float , pi : Optional [np .ndarray ]):
259289 """
0 commit comments