Make SimpleModel immutable (#5760)

Author: biotinker

components/gantry/multiaxis/multiaxis.go

@@ -92,15 +92,18 @@ func newMultiAxis(
 		return nil, err
 	}
 
-	model := referenceframe.NewSimpleModel(mAx.Name().Name)
+	frames := make([]referenceframe.Frame, 0, len(mAx.subAxes))
 	for _, subAx := range mAx.subAxes {
 		k, err := subAx.Kinematics(ctx)
 		if err != nil {
 			return nil, err
 		}
-		model.SetOrdTransforms(append(model.OrdTransforms(), k))
+		frames = append(frames, k)
+	}
+	mAx.model, err = referenceframe.NewSerialModel(mAx.Name().Name, frames)
+	if err != nil {
+		return nil, err
 	}
-	mAx.model = model
 
 	return mAx, nil
 }

components/gantry/multiaxis/multiaxis_test.go

@@ -17,8 +17,8 @@ import (
 	rutils "go.viam.com/rdk/utils"
 )
 
-func createFakeOneaAxis(length float64, positions []float64) *inject.Gantry {
-	fakesingleaxis := inject.NewGantry("fake")
+func createFakeOneaAxis(name string, length float64, positions []float64) *inject.Gantry {
+	fakesingleaxis := inject.NewGantry(name)
 	fakesingleaxis.PositionFunc = func(ctx context.Context, extra map[string]interface{}) ([]float64, error) {
 		return positions, nil
 	}
@@ -38,7 +38,7 @@ func createFakeOneaAxis(length float64, positions []float64) *inject.Gantry {
 		return nil
 	}
 	fakesingleaxis.KinematicsFunc = func(ctx context.Context) (referenceframe.Model, error) {
-		return referenceframe.NewSimpleModel(""), nil
+		return referenceframe.NewSimpleModel(name), nil
 	}
 	return fakesingleaxis
 }
@@ -49,21 +49,21 @@ func createFakeDeps() resource.Dependencies {
 		return []float64{1}, nil
 	}
 	fakeGantry1.KinematicsFunc = func(ctx context.Context) (referenceframe.Model, error) {
-		return referenceframe.NewSimpleModel(""), nil
+		return referenceframe.NewSimpleModel("1"), nil
 	}
 	fakeGantry2 := inject.NewGantry("2")
 	fakeGantry2.LengthsFunc = func(ctx context.Context, extra map[string]interface{}) ([]float64, error) {
 		return []float64{1}, nil
 	}
 	fakeGantry2.KinematicsFunc = func(ctx context.Context) (referenceframe.Model, error) {
-		return referenceframe.NewSimpleModel(""), nil
+		return referenceframe.NewSimpleModel("2"), nil
 	}
 	fakeGantry3 := inject.NewGantry("3")
 	fakeGantry3.LengthsFunc = func(ctx context.Context, extra map[string]interface{}) ([]float64, error) {
 		return []float64{1}, nil
 	}
 	fakeGantry3.KinematicsFunc = func(ctx context.Context) (referenceframe.Model, error) {
-		return referenceframe.NewSimpleModel(""), nil
+		return referenceframe.NewSimpleModel("3"), nil
 	}
 	fakeMotor := &fm.Motor{
 		Named: motor.Named("fm1").AsNamed(),
@@ -78,14 +78,14 @@ func createFakeDeps() resource.Dependencies {
 }
 
 var threeAxes = []gantry.Gantry{
-	createFakeOneaAxis(1, []float64{1}),
-	createFakeOneaAxis(2, []float64{5}),
-	createFakeOneaAxis(3, []float64{9}),
+	createFakeOneaAxis("axis1", 1, []float64{1}),
+	createFakeOneaAxis("axis2", 2, []float64{5}),
+	createFakeOneaAxis("axis3", 3, []float64{9}),
 }
 
 var twoAxes = []gantry.Gantry{
-	createFakeOneaAxis(5, []float64{1}),
-	createFakeOneaAxis(6, []float64{5}),
+	createFakeOneaAxis("axis5", 5, []float64{1}),
+	createFakeOneaAxis("axis6", 6, []float64{5}),
 }
 
 func TestValidate(t *testing.T) {

referenceframe/errors.go

@@ -78,3 +78,9 @@ func NewParentFrameNotInMapOfParentsError(parentFrameName string) error {
 func NewReservedWordError(configType, reservedWord string) error {
 	return errors.Errorf("reserved word: cannot name a %s '%s'", configType, reservedWord)
 }
+
+// NewDuplicateFrameNameError returns an error indicating that multiple frames
+// with the same name were provided.
+func NewDuplicateFrameNameError(frameName string) error {
+	return errors.Errorf("duplicate frame name %q in serial model", frameName)
+}

referenceframe/frame.go

@@ -815,8 +815,8 @@ func framesAlmostEqual(frame1, frame2 Frame, epsilon float64) (bool, error) {
 		}
 	case *SimpleModel:
 		f2 := frame2.(*SimpleModel)
-		ordTransforms1 := f1.OrdTransforms()
-		ordTransforms2 := f2.OrdTransforms()
+		ordTransforms1 := f1.ordTransforms
+		ordTransforms2 := f2.ordTransforms
 		if len(ordTransforms1) != len(ordTransforms2) {
 			return false, nil
 		} else {
@@ -836,8 +836,8 @@ func framesAlmostEqual(frame1, frame2 Frame, epsilon float64) (bool, error) {
 	return true, nil
 }
 
-// Clone makes a copy of a Frame.
-func Clone(f Frame) (Frame, error) {
+// clone makes a copy of a Frame.
+func clone(f Frame) (Frame, error) {
 	t := reflect.TypeOf(f)
 	var newFrame Frame
 

referenceframe/frame_system_test.go

@@ -106,9 +106,9 @@ func TestFrameModelPart(t *testing.T) {
 	test.That(t, partAgain.FrameConfig.pose, test.ShouldResemble, part.FrameConfig.pose)
 	test.That(t, partAgain.ModelFrame.Name, test.ShouldEqual, part.ModelFrame.Name)
 	test.That(t,
-		len(partAgain.ModelFrame.(*SimpleModel).OrdTransforms()),
+		len(partAgain.ModelFrame.DoF()),
 		test.ShouldEqual,
-		len(part.ModelFrame.(*SimpleModel).OrdTransforms()),
+		len(part.ModelFrame.DoF()),
 	)
 }
 

referenceframe/linear_inputs_test.go

@@ -54,31 +54,27 @@ func TestLinearInputsLimits(t *testing.T) {
 	// frame and a 2dof simple model. We envision the 2dof simple model as an arm with 2 joints and
 	// 3 links.
 	fs := NewEmptyFrameSystem("fs")
-	fs.AddFrame(NewZeroStaticFrame("0dof"), fs.World())
+	err := fs.AddFrame(NewZeroStaticFrame("0dof"), fs.World())
+	test.That(t, err, test.ShouldBeNil)
 
 	rotFrame, err := NewRotationalFrame("1dof", spatial.R4AA{RX: 1, RY: 0, RZ: 0}, Limit{-10, 10})
 	test.That(t, err, test.ShouldBeNil)
-	fs.AddFrame(rotFrame, fs.World())
+	err = fs.AddFrame(rotFrame, fs.World())
+	test.That(t, err, test.ShouldBeNil)
 
-	// Dan: It's unclear my by-hand construction of an arm and adding it to the frame system is
-	// completely kosher. For the purpose of this test though, I see it behaving as I need it to.
 	baseArmFrame := NewZeroStaticFrame("base")
 	shoulderArmFrame, err := NewRotationalFrame("shoulder", spatial.R4AA{RX: 1, RY: 0, RZ: 0}, Limit{-10, 10})
 	test.That(t, err, test.ShouldBeNil)
 	upperArmFrame := NewZeroStaticFrame("upperArm")
-	elbowArmFrame, err := NewRotationalFrame("shoulder", spatial.R4AA{RX: 1, RY: 0, RZ: 0}, Limit{-10, 10})
+	elbowArmFrame, err := NewRotationalFrame("elbow", spatial.R4AA{RX: 1, RY: 0, RZ: 0}, Limit{-10, 10})
 	test.That(t, err, test.ShouldBeNil)
 	handArmFrame := NewZeroStaticFrame("hand")
-	armFrame := NewSimpleModel("arm")
-	armFrame.SetOrdTransforms([]Frame{
+	armFrame, err := NewSerialModel("arm", []Frame{
 		baseArmFrame, shoulderArmFrame, upperArmFrame, elbowArmFrame, handArmFrame,
 	})
-	fs.AddFrame(armFrame, fs.World())
-	fs.AddFrame(baseArmFrame, armFrame)
-	fs.AddFrame(shoulderArmFrame, baseArmFrame)
-	fs.AddFrame(upperArmFrame, shoulderArmFrame)
-	fs.AddFrame(elbowArmFrame, upperArmFrame)
-	fs.AddFrame(handArmFrame, elbowArmFrame)
+	test.That(t, err, test.ShouldBeNil)
+	err = fs.AddFrame(armFrame, fs.World())
+	test.That(t, err, test.ShouldBeNil)
 
 	// Create inputs, do a transform call that succeeds.
 	li := NewLinearInputs()

referenceframe/model.go

@@ -24,7 +24,6 @@ import (
 type Model interface {
 	Frame
 	ModelConfig() *ModelConfigJSON
-	ModelPieceFrames([]Input) (map[string]Frame, error)
 }
 
 // KinematicModelFromProtobuf returns a model from a protobuf message representing it.
@@ -137,6 +136,78 @@ func NewSimpleModel(name string) *SimpleModel {
 	}
 }
 
+// NewSerialModel is a convenience constructor that builds a Model from a serial chain of frames.
+// Returns an error if duplicate frame names are detected.
+func NewSerialModel(name string, frames []Frame) (*SimpleModel, error) {
+	seen := make(map[string]bool)
+	for _, f := range frames {
+		frameName := f.Name()
+		if seen[frameName] {
+			return nil, NewDuplicateFrameNameError(frameName)
+		}
+		seen[frameName] = true
+	}
+
+	m := NewSimpleModel(name)
+	m.setOrdTransforms(frames)
+	return m, nil
+}
+
+// NewModelWithLimitOverrides constructs a new model identical to base but with the specified
+// joint limits overridden. Overrides are keyed by frame name. Each override replaces the
+// first DoF limit of the matching frame.
+func NewModelWithLimitOverrides(base *SimpleModel, overrides map[string]Limit) (*SimpleModel, error) {
+	clonedFrames := make([]Frame, len(base.ordTransforms))
+	for i, f := range base.ordTransforms {
+		cloned, err := clone(f)
+		if err != nil {
+			return nil, fmt.Errorf("cloning frame %q: %w", f.Name(), err)
+		}
+		clonedFrames[i] = cloned
+	}
+
+	for name, limit := range overrides {
+		found := false
+		for _, f := range clonedFrames {
+			if f.Name() == name && len(f.DoF()) > 0 {
+				f.DoF()[0] = limit
+				found = true
+				break
+			}
+		}
+		if !found {
+			return nil, fmt.Errorf("frame %q not found or has no DoF", name)
+		}
+	}
+
+	m := NewSimpleModel(base.name)
+	m.setOrdTransforms(clonedFrames)
+	m.modelConfig = base.modelConfig
+	return m, nil
+}
+
+// MoveableFrameNames returns the names of frames with non-zero DoF, in order.
+func (m *SimpleModel) MoveableFrameNames() []string {
+	var names []string
+	for _, f := range m.ordTransforms {
+		if len(f.DoF()) > 0 {
+			names = append(names, f.Name())
+		}
+	}
+	return names
+}
+
+// setOrdTransforms sets the internal ordered transforms and recomputes limits.
+func (m *SimpleModel) setOrdTransforms(fs []Frame) {
+	m.ordTransforms = fs
+	m.limits = []Limit{}
+	for _, transform := range m.ordTransforms {
+		if len(transform.DoF()) > 0 {
+			m.limits = append(m.limits, transform.DoF()...)
+		}
+	}
+}
+
 // GenerateRandomConfiguration generates a list of radian joint positions that are random but valid for each joint.
 func GenerateRandomConfiguration(m Model, randSeed *rand.Rand) []float64 {
 	limits := m.DoF()
@@ -176,7 +247,7 @@ func (m *SimpleModel) Transform(inputs []Input) (spatialmath.Pose, error) {
 func (m *SimpleModel) Interpolate(from, to []Input, by float64) ([]Input, error) {
 	interp := make([]Input, 0, len(from))
 	posIdx := 0
-	for _, transform := range m.OrdTransforms() {
+	for _, transform := range m.ordTransforms {
 		dof := len(transform.DoF()) + posIdx
 		fromSubset := from[posIdx:dof]
 		toSubset := to[posIdx:dof]
@@ -191,36 +262,28 @@ func (m *SimpleModel) Interpolate(from, to []Input, by float64) ([]Input, error)
 	return interp, nil
 }
 
-// OrdTransforms gets the OrdTransforms.
-func (m *SimpleModel) OrdTransforms() []Frame {
-	return m.ordTransforms
-}
-
 // InputFromProtobuf converts pb.JointPosition to inputs.
 func (m *SimpleModel) InputFromProtobuf(jp *pb.JointPositions) []Input {
 	inputs := make([]Input, 0, len(jp.Values))
 	posIdx := 0
-	for _, transform := range m.OrdTransforms() {
+	for _, transform := range m.ordTransforms {
 		dof := len(transform.DoF()) + posIdx
 		jPos := jp.Values[posIdx:dof]
 		posIdx = dof
-
 		inputs = append(inputs, transform.InputFromProtobuf(&pb.JointPositions{Values: jPos})...)
 	}
-
 	return inputs
 }
 
 // ProtobufFromInput converts inputs to pb.JointPosition.
 func (m *SimpleModel) ProtobufFromInput(input []Input) *pb.JointPositions {
 	jPos := &pb.JointPositions{}
 	posIdx := 0
-	for _, transform := range m.OrdTransforms() {
+	for _, transform := range m.ordTransforms {
 		dof := len(transform.DoF()) + posIdx
 		jPos.Values = append(jPos.Values, transform.ProtobufFromInput(input[posIdx:dof]).Values...)
 		posIdx = dof
 	}
-
 	return jPos
 }
 
@@ -271,22 +334,11 @@ func (m *SimpleModel) DoF() []Limit {
 	return m.limits
 }
 
-// SetOrdTransforms sets the ordTransforms.
-func (m *SimpleModel) SetOrdTransforms(fs []Frame) {
-	m.ordTransforms = fs
-	m.limits = []Limit{}
-	for _, transform := range m.ordTransforms {
-		if len(transform.DoF()) > 0 {
-			m.limits = append(m.limits, transform.DoF()...)
-		}
-	}
-}
-
 // MarshalJSON serializes a Model.
 func (m *SimpleModel) MarshalJSON() ([]byte, error) {
 	type serialized struct {
 		Name   string           `json:"name"`
-		Model  *ModelConfigJSON `json:"model"`
+		Model  *ModelConfigJSON `json:"model,omitempty"`
 		Limits []Limit          `json:"limits"`
 	}
 	ser := serialized{
@@ -301,7 +353,7 @@ func (m *SimpleModel) MarshalJSON() ([]byte, error) {
 func (m *SimpleModel) UnmarshalJSON(data []byte) error {
 	type serialized struct {
 		Name   string           `json:"name"`
-		Model  *ModelConfigJSON `json:"model"`
+		Model  *ModelConfigJSON `json:"model,omitempty"`
 		Limits []Limit          `json:"limits"`
 	}
 	var ser serialized
@@ -323,28 +375,14 @@ func (m *SimpleModel) UnmarshalJSON(data []byte) error {
 		if !ok {
 			return fmt.Errorf("could not parse config for simple model, name: %v", ser.Name)
 		}
-		m.SetOrdTransforms(newModel.OrdTransforms())
+		m.ordTransforms = newModel.ordTransforms
 	}
 	m.baseFrame = baseFrame{name: frameName, limits: ser.Limits}
 	m.modelConfig = ser.Model
 
 	return nil
 }
 
-// ModelPieceFrames takes a list of inputs and returns a map of frame names to their corresponding static frames,
-// effectively breaking the model into its kinematic pieces.
-func (m *SimpleModel) ModelPieceFrames(inputs []Input) (map[string]Frame, error) {
-	poses, err := m.inputsToFrames(inputs, true)
-	if err != nil {
-		return nil, err
-	}
-	frameMap := map[string]Frame{}
-	for _, sFrame := range poses {
-		frameMap[sFrame.Name()] = sFrame
-	}
-	return frameMap, nil
-}
-
 // inputsToFrames takes a model and a list of joint angles in radians and computes the dual quaternion representing the
 // cartesian position of each of the links up to and including the end effector. This is useful for when conversions
 // between quaternions and OV are not needed.
@@ -353,7 +391,7 @@ func (m *SimpleModel) inputsToFrames(inputs []Input, collectAll bool) ([]*static
 		return nil, NewIncorrectDoFError(len(inputs), len(m.DoF()))
 	}
 
-	poses := make([]*staticFrame, 0, len(m.OrdTransforms()))
+	poses := make([]*staticFrame, 0, len(m.ordTransforms))
 	// Start at ((1+0i+0j+0k)+(+0+0i+0j+0k)ϵ)
 	composedTransformation := spatialmath.NewZeroPose()
 	posIdx := 0
@@ -505,15 +543,16 @@ func New2DMobileModelFrame(name string, limits []Limit, collisionGeometry spatia
 		return nil, err
 	}
 
-	model := NewSimpleModel(name)
+	var frames []Frame
 	if len(limits) == 3 {
 		theta, err := NewRotationalFrame("theta", *spatialmath.NewR4AA(), limits[2])
 		if err != nil {
 			return nil, err
 		}
-		model.SetOrdTransforms([]Frame{x, y, theta, geometry})
+		frames = []Frame{x, y, theta, geometry}
 	} else {
-		model.SetOrdTransforms([]Frame{x, y, geometry})
+		frames = []Frame{x, y, geometry}
 	}
-	return model, nil
+
+	return NewSerialModel(name, frames)
 }