@@ -9,31 +9,38 @@ import (
99 "go.viam.com/rdk/referenceframe"
1010)
1111
12- // return is floats from [0-1] given a percentage of their input range that should be searched
13- // for example, if the frame system has 2 arms, and only is moving, the inputs for the non-moving arm will all be 0
14- // the other arm will be scaled 0-1 based on the expected joint distance
15- // there is a chacne it's not enough and will need be moved more.
16- func inputChangeRatio (
12+ // searchHeadroom is the multiplier applied to raw joint sensitivity ratios when computing
13+ // search ranges. A value of 5 means we search 5x the estimated joint range needed to reach
14+ // the goal, providing headroom for the non-linearity of the distance function.
15+ const searchHeadroom = 5.0
16+
17+ // computeJointSensitivities returns a per-joint sensitivity ratio indicating what fraction of each
18+ // joint's total range is estimated to be needed to reach the goal. Non-moving joints are indicated
19+ // with a value of -1. Use clampSensitivities to convert raw ratios into usable search bounds.
20+ //
21+ // For each moving joint, a small perturbation (1% of range) is applied to estimate sensitivity.
22+ func computeJointSensitivities (
1723 mc * motionChains ,
1824 startNotMine * referenceframe.LinearInputs ,
1925 frameSystem * referenceframe.FrameSystem ,
2026 distanceFunc motionplan.StateFSMetric ,
21- minJog float64 ,
2227 logger logging.Logger ,
2328) ([]float64 , error ) {
2429 inputsSchema , err := startNotMine .GetSchema (frameSystem )
2530 if err != nil {
2631 return nil , err
2732 }
2833
29- ratios := []float64 {}
34+ rawRatios := []float64 {}
3035
3136 // Sorry for the hacky copy.
3237 start := startNotMine .ToFrameSystemInputs ().ToLinearInputs ()
3338 _ , nonmoving := mc .framesFilteredByMovingAndNonmoving ()
3439 startDistance := distanceFunc (& motionplan.StateFS {Configuration : startNotMine , FS : mc .fs })
3540 logger .Debugf ("startDistance: %0.2f" , startDistance )
3641
42+ const percentJog = 0.01
43+
3744 for _ , frameName := range inputsSchema .FrameNamesInOrder () {
3845 frame := frameSystem .Frame (frameName )
3946 if len (frame .DoF ()) == 0 {
@@ -44,22 +51,22 @@ func inputChangeRatio(
4451 if slices .Contains (nonmoving , frame .Name ()) {
4552 // Frames that can move, but we are not moving them to solve this problem.
4653 for range frame .DoF () {
47- ratios = append (ratios , 0 )
54+ rawRatios = append (rawRatios , - 1 )
4855 }
4956 continue
5057 }
51- const percentJog = 0.01
5258
5359 // For each degree of freedom, we want to determine how much impact a small change
5460 // makes. For cases where a small movement results in a big change in distance, we want to
5561 // walk in smaller steps. For cases where a small change has a small effect, we want to
5662 // allow the walking algorithm to take bigger steps.
5763 for idx := range frame .DoF () {
64+ linearIdx := len (rawRatios )
5865 orig := start .Get (frame .Name ())[idx ]
5966
6067 // Compute the new input for a specific joint that's one "jog" away. E.g: ~5 degrees for
6168 // a rotational joint.
62- y := inputsSchema .Jog (len ( ratios ) , orig , percentJog )
69+ y := inputsSchema .Jog (linearIdx , orig , percentJog )
6370
6471 // Update the copied joint set in place. This is undone at the end of the loop.
6572 start .Get (frame .Name ())[idx ] = y
@@ -69,28 +76,38 @@ func inputChangeRatio(
6976 // the ratio.
7077 //
7178 // Note that Go deals with the potential divide by 0. Representing `thisRatio` as
72- // infinite. The following comparisons continue to work as expected. Resulting in an
73- // adjusted jog ratio of 1.
79+ // infinite. The following comparisons continue to work as expected.
7480 thisRatio := startDistance / math .Abs (myDistance - startDistance )
7581 myJogRatio := percentJog * thisRatio
76- // For movable frames/joints, 0.03 is the actual smallest value we'll use.
77- adjustedJogRatio := min (1 , max (minJog , (myJogRatio * 5 )))
78-
79- if math .IsNaN (adjustedJogRatio ) {
80- adjustedJogRatio = 1
81- }
8282
83- logger .Debugf ("idx: %d myDistance: %0.2f thisRatio: %0.3f myJogRatio: %0.3f adjustJogRatio: %0.3f " ,
84- idx , myDistance , thisRatio , myJogRatio , adjustedJogRatio )
83+ logger .Debugf ("idx: %d myDistance: %0.2f thisRatio: %0.3f myJogRatio: %0.3f" ,
84+ linearIdx , myDistance , thisRatio , myJogRatio )
8585
86- ratios = append (ratios , adjustedJogRatio )
86+ rawRatios = append (rawRatios , myJogRatio )
8787
8888 // Undo the above modification. Returning `start` back to its original state.
8989 start .Get (frame .Name ())[idx ] = orig
9090 }
9191 }
9292
93- logger .Debugf ("inputChangeRatio result: %v" , ratios )
93+ logger .Debugf ("computeJointSensitivities result: %v" , rawRatios )
94+ return rawRatios , nil
95+ }
9496
95- return ratios , nil
97+ // clampSensitivities converts raw per-joint sensitivity ratios (from computeJointSensitivities)
98+ // into bounded search range ratios. Non-moving joints (indicated by -1) are set to 0.
99+ // Moving joints are scaled by searchHeadroom and clamped to [minJog, 1.0].
100+ func clampSensitivities (rawRatios []float64 , minJog float64 ) []float64 {
101+ ratios := make ([]float64 , len (rawRatios ))
102+ for i , raw := range rawRatios {
103+ if raw < 0 {
104+ // Non-moving joint sentinel.
105+ continue
106+ }
107+ ratios [i ] = min (1 , max (minJog , raw * searchHeadroom ))
108+ if math .IsNaN (ratios [i ]) {
109+ ratios [i ] = 1
110+ }
111+ }
112+ return ratios
96113}
0 commit comments