From 7d74b0718bf45db099c159f2c6e4b88e800a5329 Mon Sep 17 00:00:00 2001 From: William Spies Date: Fri, 7 Apr 2023 12:08:44 -0600 Subject: [PATCH 1/4] Update exposed constraint names to be more descriptive --- motionplan/constraint.go | 6 +++--- motionplan/constraint_test.go | 4 ++-- motionplan/planManager.go | 6 +++--- motionplan/plannerOptions.go | 19 +++++++++---------- 4 files changed, 17 insertions(+), 18 deletions(-) diff --git a/motionplan/constraint.go b/motionplan/constraint.go index 87f901a4133..afa3b82fa8c 100644 --- a/motionplan/constraint.go +++ b/motionplan/constraint.go @@ -301,9 +301,9 @@ func createAllCollisionConstraints( return nil, err } return map[string]StateConstraint{ - defaultObstacleConstraintName: obstacleConstraint, - defaultSelfCollisionConstraintName: selfCollisionConstraint, - defaultRobotCollisionConstraintName: robotConstraint, + defaultObstacleConstraintDesc: obstacleConstraint, + defaultSelfCollisionConstraintDesc: selfCollisionConstraint, + defaultRobotCollisionConstraintDesc: robotConstraint, }, nil } diff --git a/motionplan/constraint_test.go b/motionplan/constraint_test.go index 228073eb53f..10aa2cc7640 100644 --- a/motionplan/constraint_test.go +++ b/motionplan/constraint_test.go @@ -195,8 +195,8 @@ func TestCollisionConstraints(t *testing.T) { }{ {zeroPos, true, ""}, {frame.FloatsToInputs([]float64{math.Pi / 2, 0, 0, 0, 0, 0}), true, ""}, - {frame.FloatsToInputs([]float64{math.Pi, 0, 0, 0, 0, 0}), false, defaultObstacleConstraintName}, - {frame.FloatsToInputs([]float64{math.Pi / 2, 0, 0, 0, 2, 0}), false, defaultSelfCollisionConstraintName}, + {frame.FloatsToInputs([]float64{math.Pi, 0, 0, 0, 0, 0}), false, defaultObstacleConstraintDesc}, + {frame.FloatsToInputs([]float64{math.Pi / 2, 0, 0, 0, 2, 0}), false, defaultSelfCollisionConstraintDesc}, } // define external obstacles diff --git a/motionplan/planManager.go b/motionplan/planManager.go index b34db8a5777..290ad9690b6 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -491,7 +491,7 @@ func (pm *planManager) plannerSetupFromMoveRequest( orientTol = defaultOrientationDeviation } constraint, pathMetric := NewAbsoluteLinearInterpolatingConstraint(from, to, linTol, orientTol) - opt.AddStateConstraint(defaultLinearConstraintName, constraint) + opt.AddStateConstraint(defaultLinearConstraintDesc, constraint) opt.pathMetric = pathMetric case PseudolinearMotionProfile: tolerance, ok := planningOpts["tolerance"].(float64) @@ -500,7 +500,7 @@ func (pm *planManager) plannerSetupFromMoveRequest( tolerance = defaultPseudolinearTolerance } constraint, pathMetric := NewProportionalLinearInterpolatingConstraint(from, to, tolerance) - opt.AddStateConstraint(defaultPseudolinearConstraintName, constraint) + opt.AddStateConstraint(defaultPseudolinearConstraintDesc, constraint) opt.pathMetric = pathMetric case OrientationMotionProfile: tolerance, ok := planningOpts["tolerance"].(float64) @@ -509,7 +509,7 @@ func (pm *planManager) plannerSetupFromMoveRequest( tolerance = defaultOrientationDeviation } constraint, pathMetric := NewSlerpOrientationConstraint(from, to, tolerance) - opt.AddStateConstraint(defaultOrientationConstraintName, constraint) + opt.AddStateConstraint(defaultOrientationConstraintDesc, constraint) opt.pathMetric = pathMetric case PositionOnlyMotionProfile: opt.SetGoalMetric(NewPositionOnlyMetric(to)) diff --git a/motionplan/plannerOptions.go b/motionplan/plannerOptions.go index d15d23d910d..f75775f04b8 100644 --- a/motionplan/plannerOptions.go +++ b/motionplan/plannerOptions.go @@ -39,14 +39,13 @@ const ( // default number of times to try to smooth the path. defaultSmoothIter = 20 - // names of constraints. - defaultLinearConstraintName = "defaultLinearConstraint" - defaultPseudolinearConstraintName = "defaultPseudolinearConstraint" - defaultOrientationConstraintName = "defaultOrientationConstraint" - defaultObstacleConstraintName = "defaultObstacleConstraint" - defaultSelfCollisionConstraintName = "defaultSelfCollisionConstraint" - defaultRobotCollisionConstraintName = "defaultRobotCollisionConstraint" - defaultJointConstraint = "defaultJointSwingConstraint" + // descriptions of constraints. + defaultLinearConstraintDesc = "Constraint to follow linear path" + defaultPseudolinearConstraintDesc = "Constraint to follow pseudolinear path, with tolerance scaled to path length" + defaultOrientationConstraintDesc = "Constraint to maintain orientation within bounds" + defaultObstacleConstraintDesc = "Collision between the robot and an obstacle" + defaultSelfCollisionConstraintDesc = "Collision between components that are moving" + defaultRobotCollisionConstraintDesc = "Collision between different pieces of the robot" // When breaking down a path into smaller waypoints, add a waypoint every this many mm of movement. defaultPathStepSize = 10 @@ -178,7 +177,7 @@ func (p *plannerOptions) addPbLinearConstraints(from, to spatialmath.Pose, pbCon orientTol = defaultOrientationDeviation } constraint, pathDist := NewAbsoluteLinearInterpolatingConstraint(from, to, float64(linTol), float64(orientTol)) - p.AddStateConstraint(defaultLinearConstraintName, constraint) + p.AddStateConstraint(defaultLinearConstraintDesc, constraint) p.pathMetric = CombineMetrics(p.pathMetric, pathDist) } @@ -189,6 +188,6 @@ func (p *plannerOptions) addPbOrientationConstraints(from, to spatialmath.Pose, orientTol = defaultOrientationDeviation } constraint, pathDist := NewSlerpOrientationConstraint(from, to, float64(orientTol)) - p.AddStateConstraint(defaultLinearConstraintName, constraint) + p.AddStateConstraint(defaultLinearConstraintDesc, constraint) p.pathMetric = CombineMetrics(p.pathMetric, pathDist) } From 4e34480c40332b031c2e3f4073a6436e3014a32c Mon Sep 17 00:00:00 2001 From: William Spies Date: Fri, 7 Apr 2023 16:26:06 -0600 Subject: [PATCH 2/4] Remove RobotConstructor from arms --- components/arm/arm.go | 34 ++++----------------- components/arm/arm_test.go | 25 +++------------ components/arm/eva/eva.go | 11 +++---- components/arm/fake/fake.go | 11 +++---- components/arm/universalrobots/ur.go | 11 +++---- components/arm/universalrobots/ur5e_test.go | 4 +-- components/arm/wrapper/wrapper.go | 26 +++++++++------- components/arm/wrapper/wrapper_test.go | 1 - components/arm/xarm/xarm.go | 14 ++++----- components/arm/xarm/xarm_comm.go | 2 +- components/arm/yahboom/dofbot.go | 13 +++----- examples/simpleserver/cmd.go | 3 +- 12 files changed, 51 insertions(+), 104 deletions(-) diff --git a/components/arm/arm.go b/components/arm/arm.go index 3e3d498563d..c733a79fa20 100644 --- a/components/arm/arm.go +++ b/components/arm/arm.go @@ -22,7 +22,6 @@ import ( "go.viam.com/rdk/registry" "go.viam.com/rdk/resource" "go.viam.com/rdk/robot" - "go.viam.com/rdk/robot/framesystem" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/subtype" "go.viam.com/rdk/utils" @@ -342,7 +341,7 @@ func WrapWithReconfigurable(r interface{}, name resource.Name) (resource.Reconfi } // Move is a helper function to abstract away movement for general arms. -func Move(ctx context.Context, r robot.Robot, a Arm, dst spatialmath.Pose) error { +func Move(ctx context.Context, logger golog.Logger, a Arm, dst spatialmath.Pose) error { joints, err := a.JointPositions(ctx, nil) if err != nil { return err @@ -356,7 +355,7 @@ func Move(ctx context.Context, r robot.Robot, a Arm, dst spatialmath.Pose) error return err } - solution, err := Plan(ctx, r, a, dst) + solution, err := Plan(ctx, logger, a, dst) if err != nil { return err } @@ -365,34 +364,13 @@ func Move(ctx context.Context, r robot.Robot, a Arm, dst spatialmath.Pose) error // Plan is a helper function to be called by arm implementations to abstract away the default procedure for using the // motion planning library with arms. -func Plan( - ctx context.Context, - r robot.Robot, - a Arm, - dst spatialmath.Pose, -) ([][]referenceframe.Input, error) { - // build the framesystem - fs, err := framesystem.RobotFrameSystem(ctx, r, nil) - if err != nil { - return nil, err - } - armName := a.ModelFrame().Name() - destination := referenceframe.NewPoseInFrame(armName+"_origin", dst) - - // PlanRobotMotion needs a frame system which contains the frame being solved for - if fs.Frame(armName) == nil { - armFrame := a.ModelFrame() - jp, err := a.JointPositions(ctx, nil) - if err != nil { - return nil, err - } - return motionplan.PlanFrameMotion(ctx, r.Logger(), dst, armFrame, armFrame.InputFromProtobuf(jp), defaultArmPlannerOptions, nil) - } - solutionMap, err := motionplan.PlanRobotMotion(ctx, destination, a.ModelFrame(), r, fs, nil, defaultArmPlannerOptions, nil) +func Plan(ctx context.Context, logger golog.Logger, a Arm, dst spatialmath.Pose) ([][]referenceframe.Input, error) { + armFrame := a.ModelFrame() + jp, err := a.JointPositions(ctx, nil) if err != nil { return nil, err } - return motionplan.FrameStepsFromRobotPath(a.ModelFrame().Name(), solutionMap) + return motionplan.PlanFrameMotion(ctx, logger, dst, armFrame, armFrame.InputFromProtobuf(jp), defaultArmPlannerOptions, nil) } // GoToWaypoints will visit in turn each of the joint position waypoints generated by a motion planner. diff --git a/components/arm/arm_test.go b/components/arm/arm_test.go index d97e9d7ee23..db7a553741f 100644 --- a/components/arm/arm_test.go +++ b/components/arm/arm_test.go @@ -23,7 +23,6 @@ import ( "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/registry" "go.viam.com/rdk/resource" - framesystemparts "go.viam.com/rdk/robot/framesystem/parts" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/testutils/inject" rutils "go.viam.com/rdk/utils" @@ -389,19 +388,7 @@ func TestOOBArm(t *testing.T) { }, } - injectedRobot := setupInjectRobot() - injectedRobot.FrameSystemConfigFunc = func( - ctx context.Context, - additionalTransforms []*referenceframe.LinkInFrame, - ) (framesystemparts.Parts, error) { - return framesystemparts.Parts{}, nil - } - - injectedRobot.LoggerFunc = func() golog.Logger { - return logger - } - - notReal, err := fake.NewArm(injectedRobot, cfg, logger) + notReal, err := fake.NewArm(cfg, logger) test.That(t, err, test.ShouldBeNil) injectedArm := &inject.Arm{ @@ -434,7 +421,7 @@ func TestOOBArm(t *testing.T) { t.Run("Move fails when OOB", func(t *testing.T) { pose = spatialmath.NewPoseFromPoint(r3.Vector{200, 200, 200}) - err := arm.Move(context.Background(), &inject.Robot{}, injectedArm, pose) + err := arm.Move(context.Background(), logger, injectedArm, pose) u := "cartesian movements are not allowed when arm joints are out of bounds" v := "joint 0 input out of bounds, input 12.56637 needs to be within range [6.28319 -6.28319]" s := strings.Join([]string{u, v}, ": ") @@ -541,9 +528,7 @@ func TestXArm6Locations(t *testing.T) { }, } - injectedRobot := setupInjectRobot() - - notReal, err := fake.NewArm(injectedRobot, cfg, logger) + notReal, err := fake.NewArm(cfg, logger) test.That(t, err, test.ShouldBeNil) t.Run("home location check", func(t *testing.T) { @@ -666,9 +651,7 @@ func TestUR5ELocations(t *testing.T) { }, } - injectedRobot := setupInjectRobot() - - notReal, err := fake.NewArm(injectedRobot, cfg, logger) + notReal, err := fake.NewArm(cfg, logger) test.That(t, err, test.ShouldBeNil) t.Run("home location check", func(t *testing.T) { diff --git a/components/arm/eva/eva.go b/components/arm/eva/eva.go index f6d20727179..09550f3f951 100644 --- a/components/arm/eva/eva.go +++ b/components/arm/eva/eva.go @@ -30,7 +30,6 @@ import ( "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/registry" "go.viam.com/rdk/resource" - "go.viam.com/rdk/robot" "go.viam.com/rdk/spatialmath" ) @@ -48,8 +47,8 @@ var evamodeljson []byte func init() { registry.RegisterComponent(arm.Subtype, ModelName, registry.Component{ - RobotConstructor: func(ctx context.Context, r robot.Robot, config config.Component, logger golog.Logger) (interface{}, error) { - return NewEva(ctx, r, config, logger) + Constructor: func(ctx context.Context, _ registry.Dependencies, config config.Component, logger golog.Logger) (interface{}, error) { + return NewEva(ctx, config, logger) }, }) @@ -97,7 +96,6 @@ type eva struct { moveLock *sync.Mutex logger golog.Logger model referenceframe.Model - robot robot.Robot frameJSON []byte @@ -105,7 +103,7 @@ type eva struct { } // NewEva TODO. -func NewEva(ctx context.Context, r robot.Robot, cfg config.Component, logger golog.Logger) (arm.LocalArm, error) { +func NewEva(ctx context.Context, cfg config.Component, logger golog.Logger) (arm.LocalArm, error) { model, err := Model(cfg.Name) if err != nil { return nil, err @@ -118,7 +116,6 @@ func NewEva(ctx context.Context, r robot.Robot, cfg config.Component, logger gol logger: logger, moveLock: &sync.Mutex{}, model: model, - robot: r, frameJSON: evamodeljson, } @@ -153,7 +150,7 @@ func (e *eva) EndPosition(ctx context.Context, extra map[string]interface{}) (sp func (e *eva) MoveToPosition(ctx context.Context, pos spatialmath.Pose, extra map[string]interface{}) error { ctx, done := e.opMgr.New(ctx) defer done() - return arm.Move(ctx, e.robot, e, pos) + return arm.Move(ctx, e.logger, e, pos) } func (e *eva) MoveToJointPositions(ctx context.Context, newPositions *pb.JointPositions, extra map[string]interface{}) error { diff --git a/components/arm/fake/fake.go b/components/arm/fake/fake.go index b969eda0156..bb6627e9274 100644 --- a/components/arm/fake/fake.go +++ b/components/arm/fake/fake.go @@ -19,7 +19,6 @@ import ( "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/registry" "go.viam.com/rdk/resource" - "go.viam.com/rdk/robot" "go.viam.com/rdk/spatialmath" ) @@ -68,8 +67,8 @@ func (config *AttrConfig) Validate(path string) error { func init() { registry.RegisterComponent(arm.Subtype, ModelName, registry.Component{ - RobotConstructor: func(ctx context.Context, r robot.Robot, config config.Component, logger golog.Logger) (interface{}, error) { - return NewArm(r, config, logger) + Constructor: func(ctx context.Context, _ registry.Dependencies, config config.Component, logger golog.Logger) (interface{}, error) { + return NewArm(config, logger) }, }) @@ -85,7 +84,7 @@ func init() { } // NewArm returns a new fake arm. -func NewArm(r robot.Robot, cfg config.Component, logger golog.Logger) (arm.LocalArm, error) { +func NewArm(cfg config.Component, logger golog.Logger) (arm.LocalArm, error) { model, err := buildModel(cfg) if err != nil { return nil, err @@ -96,7 +95,6 @@ func NewArm(r robot.Robot, cfg config.Component, logger golog.Logger) (arm.Local joints: &pb.JointPositions{Values: make([]float64, len(model.DoF()))}, model: model, logger: logger, - robot: r, }, nil } @@ -131,7 +129,6 @@ type Arm struct { CloseCount int logger golog.Logger model referenceframe.Model - robot robot.Robot } // UpdateAction helps hinting the reconfiguration process on what strategy to use given a modified config. @@ -170,7 +167,7 @@ func (a *Arm) EndPosition(ctx context.Context, extra map[string]interface{}) (sp // MoveToPosition sets the position. func (a *Arm) MoveToPosition(ctx context.Context, pos spatialmath.Pose, extra map[string]interface{}) error { - return arm.Move(ctx, a.robot, a, pos) + return arm.Move(ctx, a.logger, a, pos) } // MoveToJointPositions sets the joints. diff --git a/components/arm/universalrobots/ur.go b/components/arm/universalrobots/ur.go index 56eaed141d6..5b85f485a0d 100644 --- a/components/arm/universalrobots/ur.go +++ b/components/arm/universalrobots/ur.go @@ -31,7 +31,6 @@ import ( "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/registry" "go.viam.com/rdk/resource" - "go.viam.com/rdk/robot" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/utils" ) @@ -62,8 +61,8 @@ var ur5modeljson []byte func init() { registry.RegisterComponent(arm.Subtype, ModelName, registry.Component{ - RobotConstructor: func(ctx context.Context, r robot.Robot, config config.Component, logger golog.Logger) (interface{}, error) { - return URArmConnect(ctx, r, config, logger) + Constructor: func(ctx context.Context, _ registry.Dependencies, config config.Component, logger golog.Logger) (interface{}, error) { + return URArmConnect(ctx, config, logger) }, }) @@ -97,7 +96,6 @@ type URArm struct { activeBackgroundWorkers *sync.WaitGroup model referenceframe.Model opMgr operation.SingleOperationManager - robot robot.Robot urHostedKinematics bool inRemoteMode bool dashboardConnection net.Conn @@ -157,7 +155,7 @@ func (ua *URArm) Close(ctx context.Context) error { } // URArmConnect TODO. -func URArmConnect(ctx context.Context, r robot.Robot, cfg config.Component, logger golog.Logger) (arm.LocalArm, error) { +func URArmConnect(ctx context.Context, cfg config.Component, logger golog.Logger) (arm.LocalArm, error) { // this is to speed up component build failure if the UR arm is not reachable ctx, cancel := context.WithDeadline(ctx, time.Now().Add(5*time.Second)) defer cancel() @@ -196,7 +194,6 @@ func URArmConnect(ctx context.Context, r robot.Robot, cfg config.Component, logg logger: logger, cancel: cancel, model: model, - robot: r, urHostedKinematics: attrs.ArmHostedKinematics, inRemoteMode: false, readRobotStateConnection: connReadRobotState, @@ -360,7 +357,7 @@ func (ua *URArm) MoveToPosition(ctx context.Context, pos spatialmath.Pose, extra if usingHostedKinematics { return ua.moveWithURHostedKinematics(ctx, pos) } - return arm.Move(ctx, ua.robot, ua, pos) + return arm.Move(ctx, ua.logger, ua, pos) } // MoveToJointPositions TODO. diff --git a/components/arm/universalrobots/ur5e_test.go b/components/arm/universalrobots/ur5e_test.go index ec2fe0808f7..2ba1136145d 100644 --- a/components/arm/universalrobots/ur5e_test.go +++ b/components/arm/universalrobots/ur5e_test.go @@ -26,7 +26,6 @@ import ( "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" "go.viam.com/rdk/spatialmath" - "go.viam.com/rdk/testutils/inject" "go.viam.com/rdk/utils" ) @@ -344,8 +343,7 @@ func TestArmReconnection(t *testing.T) { }, } - injectRobot := &inject.Robot{} - arm, err := URArmConnect(parentCtx, injectRobot, cfg, logger) + arm, err := URArmConnect(parentCtx, cfg, logger) test.That(t, err, test.ShouldBeNil) ua, ok := arm.(*URArm) diff --git a/components/arm/wrapper/wrapper.go b/components/arm/wrapper/wrapper.go index 392bd67f61c..50447a3f4b5 100644 --- a/components/arm/wrapper/wrapper.go +++ b/components/arm/wrapper/wrapper.go @@ -6,7 +6,7 @@ import ( "github.com/edaniels/golog" pb "go.viam.com/api/component/arm/v1" - "go.viam.com/utils" + goutils "go.viam.com/utils" "go.viam.com/rdk/components/arm" "go.viam.com/rdk/components/generic" @@ -16,8 +16,8 @@ import ( "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/registry" "go.viam.com/rdk/resource" - "go.viam.com/rdk/robot" "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/utils" ) // AttrConfig is used for converting config attributes. @@ -32,7 +32,7 @@ var model = resource.NewDefaultModel("wrapper_arm") func (cfg *AttrConfig) Validate(path string) ([]string, error) { var deps []string if cfg.ArmName == "" { - return nil, utils.NewConfigValidationFieldRequiredError(path, "arm-name") + return nil, goutils.NewConfigValidationFieldRequiredError(path, "arm-name") } if _, err := referenceframe.ModelFromPath(cfg.ModelFilePath, ""); err != nil { return nil, err @@ -43,8 +43,8 @@ func (cfg *AttrConfig) Validate(path string) ([]string, error) { func init() { registry.RegisterComponent(arm.Subtype, model, registry.Component{ - RobotConstructor: func(ctx context.Context, r robot.Robot, config config.Component, logger golog.Logger) (interface{}, error) { - return NewWrapperArm(config, r, logger) + Constructor: func(ctx context.Context, deps registry.Dependencies, config config.Component, logger golog.Logger) (interface{}, error) { + return NewWrapperArm(config, deps, logger) }, }) @@ -64,18 +64,23 @@ type Arm struct { model referenceframe.Model actual arm.Arm logger golog.Logger - robot robot.Robot opMgr operation.SingleOperationManager } // NewWrapperArm returns a wrapper component for another arm. -func NewWrapperArm(cfg config.Component, r robot.Robot, logger golog.Logger) (arm.LocalArm, error) { - modelPath := cfg.ConvertedAttributes.(*AttrConfig).ModelFilePath +func NewWrapperArm(cfg config.Component, deps registry.Dependencies, logger golog.Logger) (arm.LocalArm, error) { + attrs, ok := cfg.ConvertedAttributes.(*AttrConfig) + if !ok { + return nil, utils.NewUnexpectedTypeError(attrs, cfg.ConvertedAttributes) + } + + modelPath := attrs.ModelFilePath model, err := referenceframe.ModelFromPath(modelPath, cfg.Name) if err != nil { return nil, err } - wrappedArm, err := arm.FromRobot(r, cfg.ConvertedAttributes.(*AttrConfig).ArmName) + + wrappedArm, err := arm.FromDependencies(deps, attrs.ArmName) if err != nil { return nil, err } @@ -84,7 +89,6 @@ func NewWrapperArm(cfg config.Component, r robot.Robot, logger golog.Logger) (ar model: model, actual: wrappedArm, logger: logger, - robot: r, }, nil } @@ -130,7 +134,7 @@ func (wrapper *Arm) EndPosition(ctx context.Context, extra map[string]interface{ func (wrapper *Arm) MoveToPosition(ctx context.Context, pos spatialmath.Pose, extra map[string]interface{}) error { ctx, done := wrapper.opMgr.New(ctx) defer done() - return arm.Move(ctx, wrapper.robot, wrapper, pos) + return arm.Move(ctx, wrapper.logger, wrapper, pos) } // MoveToJointPositions sets the joints. diff --git a/components/arm/wrapper/wrapper_test.go b/components/arm/wrapper/wrapper_test.go index 56e26fa1d8f..a0da9c9fa0b 100644 --- a/components/arm/wrapper/wrapper_test.go +++ b/components/arm/wrapper/wrapper_test.go @@ -67,7 +67,6 @@ func TestUpdateAction(t *testing.T) { model: model, actual: &inject.Arm{}, logger: logger, - robot: &inject.Robot{}, } // scenario where we do not reconfigure diff --git a/components/arm/xarm/xarm.go b/components/arm/xarm/xarm.go index 2d4f989480c..aced9e96b52 100644 --- a/components/arm/xarm/xarm.go +++ b/components/arm/xarm/xarm.go @@ -19,7 +19,6 @@ import ( "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/registry" "go.viam.com/rdk/resource" - "go.viam.com/rdk/robot" "go.viam.com/rdk/utils" ) @@ -48,7 +47,7 @@ type xArm struct { model referenceframe.Model started bool opMgr operation.SingleOperationManager - robot robot.Robot + logger golog.Logger } //go:embed xarm6_kinematics.json @@ -78,8 +77,8 @@ func Model(name string, dof int) (referenceframe.Model, error) { func init() { // xArm6 registry.RegisterComponent(arm.Subtype, ModelName6DOF, registry.Component{ - RobotConstructor: func(ctx context.Context, r robot.Robot, config config.Component, logger golog.Logger) (interface{}, error) { - return NewxArm(ctx, r, config, logger, 6) + Constructor: func(ctx context.Context, _ registry.Dependencies, config config.Component, logger golog.Logger) (interface{}, error) { + return NewxArm(ctx, config, logger, 6) }, }) @@ -93,8 +92,8 @@ func init() { // xArm7 registry.RegisterComponent(arm.Subtype, ModelName7DOF, registry.Component{ - RobotConstructor: func(ctx context.Context, r robot.Robot, config config.Component, logger golog.Logger) (interface{}, error) { - return NewxArm(ctx, r, config, logger, 7) + Constructor: func(ctx context.Context, _ registry.Dependencies, config config.Component, logger golog.Logger) (interface{}, error) { + return NewxArm(ctx, config, logger, 7) }, }) config.RegisterComponentAttributeMapConverter(arm.Subtype, ModelName7DOF, @@ -107,7 +106,7 @@ func init() { } // NewxArm returns a new xArm with the specified dof. -func NewxArm(ctx context.Context, r robot.Robot, cfg config.Component, logger golog.Logger, dof int) (arm.LocalArm, error) { +func NewxArm(ctx context.Context, cfg config.Component, logger golog.Logger, dof int) (arm.LocalArm, error) { armCfg := cfg.ConvertedAttributes.(*AttrConfig) if armCfg.Host == "" { @@ -143,7 +142,6 @@ func NewxArm(ctx context.Context, r robot.Robot, cfg config.Component, logger go moveHZ: 100., model: model, started: false, - robot: r, } err = xA.start(ctx) diff --git a/components/arm/xarm/xarm_comm.go b/components/arm/xarm/xarm_comm.go index 8f3061df035..398055ec4b3 100644 --- a/components/arm/xarm/xarm_comm.go +++ b/components/arm/xarm/xarm_comm.go @@ -423,7 +423,7 @@ func (x *xArm) MoveToPosition(ctx context.Context, pos spatialmath.Pose, extra m return err } } - if err := arm.Move(ctx, x.robot, x, pos); err != nil { + if err := arm.Move(ctx, x.logger, x, pos); err != nil { return err } return x.opMgr.WaitForSuccess( diff --git a/components/arm/yahboom/dofbot.go b/components/arm/yahboom/dofbot.go index c947f3040a5..402f0dc03fb 100644 --- a/components/arm/yahboom/dofbot.go +++ b/components/arm/yahboom/dofbot.go @@ -26,7 +26,6 @@ import ( "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/registry" "go.viam.com/rdk/resource" - "go.viam.com/rdk/robot" "go.viam.com/rdk/spatialmath" rdkutils "go.viam.com/rdk/utils" ) @@ -93,8 +92,8 @@ func (config *AttrConfig) Validate(path string) error { func init() { registry.RegisterComponent(arm.Subtype, ModelName, registry.Component{ - RobotConstructor: func(ctx context.Context, r robot.Robot, config config.Component, logger golog.Logger) (interface{}, error) { - return NewDofBot(ctx, r, config, logger) + Constructor: func(ctx context.Context, deps registry.Dependencies, config config.Component, logger golog.Logger) (interface{}, error) { + return NewDofBot(ctx, deps, config, logger) }, }) @@ -110,7 +109,6 @@ type Dofbot struct { generic.Unimplemented handle board.I2CHandle model referenceframe.Model - robot robot.Robot mu sync.Mutex muMove sync.Mutex logger golog.Logger @@ -119,7 +117,7 @@ type Dofbot struct { } // NewDofBot is a constructor to create a new dofbot arm. -func NewDofBot(ctx context.Context, r robot.Robot, config config.Component, logger golog.Logger) (arm.LocalArm, error) { +func NewDofBot(ctx context.Context, deps registry.Dependencies, config config.Component, logger golog.Logger) (arm.LocalArm, error) { var err error attr, ok := config.ConvertedAttributes.(*AttrConfig) @@ -129,9 +127,8 @@ func NewDofBot(ctx context.Context, r robot.Robot, config config.Component, logg a := Dofbot{} a.logger = logger - a.robot = r - b, err := board.FromRobot(r, attr.Board) + b, err := board.FromDependencies(deps, attr.Board) if err != nil { return nil, err } @@ -177,7 +174,7 @@ func (a *Dofbot) EndPosition(ctx context.Context, extra map[string]interface{}) func (a *Dofbot) MoveToPosition(ctx context.Context, pos spatialmath.Pose, extra map[string]interface{}) error { ctx, done := a.opMgr.New(ctx) defer done() - return arm.Move(ctx, a.robot, a, pos) + return arm.Move(ctx, a.logger, a, pos) } // MoveToJointPositions moves the arm's joints to the given positions. diff --git a/examples/simpleserver/cmd.go b/examples/simpleserver/cmd.go index df7cb6a6d5b..5e6ad9e1a86 100644 --- a/examples/simpleserver/cmd.go +++ b/examples/simpleserver/cmd.go @@ -14,7 +14,6 @@ import ( "go.viam.com/rdk/resource" robotimpl "go.viam.com/rdk/robot/impl" "go.viam.com/rdk/robot/web" - "go.viam.com/rdk/testutils/inject" ) var logger = golog.NewDebugLogger("simpleserver") @@ -32,7 +31,7 @@ func mainWithArgs(ctx context.Context, args []string, logger golog.Logger) error ArmModel: "ur5e", }, } - arm1, err := fake.NewArm(&inject.Robot{}, cfg, logger) + arm1, err := fake.NewArm(cfg, logger) if err != nil { return err } From c27c31659dfc0284bd4c0b7e27132007114e63fc Mon Sep 17 00:00:00 2001 From: William Spies Date: Fri, 7 Apr 2023 16:58:31 -0600 Subject: [PATCH 3/4] Add a logger that was missing from xArms --- components/arm/xarm/xarm.go | 1 + 1 file changed, 1 insertion(+) diff --git a/components/arm/xarm/xarm.go b/components/arm/xarm/xarm.go index aced9e96b52..4028b65bc50 100644 --- a/components/arm/xarm/xarm.go +++ b/components/arm/xarm/xarm.go @@ -142,6 +142,7 @@ func NewxArm(ctx context.Context, cfg config.Component, logger golog.Logger, dof moveHZ: 100., model: model, started: false, + logger: logger, } err = xA.start(ctx) From 456c8900454541a7c361ea4b8eecd01634c8061c Mon Sep 17 00:00:00 2001 From: William Spies Date: Mon, 10 Apr 2023 13:03:03 -0600 Subject: [PATCH 4/4] Cleaned up constraint strings from feedback --- motionplan/plannerOptions.go | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/motionplan/plannerOptions.go b/motionplan/plannerOptions.go index f75775f04b8..01af49f9469 100644 --- a/motionplan/plannerOptions.go +++ b/motionplan/plannerOptions.go @@ -44,8 +44,8 @@ const ( defaultPseudolinearConstraintDesc = "Constraint to follow pseudolinear path, with tolerance scaled to path length" defaultOrientationConstraintDesc = "Constraint to maintain orientation within bounds" defaultObstacleConstraintDesc = "Collision between the robot and an obstacle" - defaultSelfCollisionConstraintDesc = "Collision between components that are moving" - defaultRobotCollisionConstraintDesc = "Collision between different pieces of the robot" + defaultSelfCollisionConstraintDesc = "Collision between two robot components that are moving" + defaultRobotCollisionConstraintDesc = "Collision between a robot component that is moving and one that is stationary" // When breaking down a path into smaller waypoints, add a waypoint every this many mm of movement. defaultPathStepSize = 10