From 0a226e34f73b3af766901234374901c7dcad107a Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 21 Nov 2024 11:51:01 -0500 Subject: [PATCH 01/13] remove xArm from RDK --- components/arm/fake/fake.go | 2 +- components/arm/register/register.go | 2 +- components/arm/xarm/lite6_kinematics.json | 217 ------- components/arm/xarm/xarm.go | 280 --------- components/arm/xarm/xarm6_kinematics.json | 217 ------- components/arm/xarm/xarm7_kinematics.json | 257 --------- components/arm/xarm/xarm_comm.go | 667 ---------------------- components/arm/xarm/xarm_test.go | 205 ------- go.mod | 3 + go.sum | 18 +- 10 files changed, 19 insertions(+), 1849 deletions(-) delete mode 100644 components/arm/xarm/lite6_kinematics.json delete mode 100644 components/arm/xarm/xarm.go delete mode 100644 components/arm/xarm/xarm6_kinematics.json delete mode 100644 components/arm/xarm/xarm7_kinematics.json delete mode 100644 components/arm/xarm/xarm_comm.go delete mode 100644 components/arm/xarm/xarm_test.go diff --git a/components/arm/fake/fake.go b/components/arm/fake/fake.go index 92947835cf1..58aef76158d 100644 --- a/components/arm/fake/fake.go +++ b/components/arm/fake/fake.go @@ -9,10 +9,10 @@ import ( "github.com/pkg/errors" + xarm "github.com/viam-modules/viam-ufactory-xarm/arm" "go.viam.com/rdk/components/arm" "go.viam.com/rdk/components/arm/eva" ur "go.viam.com/rdk/components/arm/universalrobots" - "go.viam.com/rdk/components/arm/xarm" "go.viam.com/rdk/logging" "go.viam.com/rdk/motionplan" "go.viam.com/rdk/referenceframe" diff --git a/components/arm/register/register.go b/components/arm/register/register.go index 23607085676..1b58a7332df 100644 --- a/components/arm/register/register.go +++ b/components/arm/register/register.go @@ -3,9 +3,9 @@ package register import ( // register arms. + _ "github.com/viam-modules/viam-ufactory-xarm/arm" _ "go.viam.com/rdk/components/arm/eva" _ "go.viam.com/rdk/components/arm/fake" _ "go.viam.com/rdk/components/arm/universalrobots" _ "go.viam.com/rdk/components/arm/wrapper" - _ "go.viam.com/rdk/components/arm/xarm" ) diff --git a/components/arm/xarm/lite6_kinematics.json b/components/arm/xarm/lite6_kinematics.json deleted file mode 100644 index 0498017440b..00000000000 --- a/components/arm/xarm/lite6_kinematics.json +++ /dev/null @@ -1,217 +0,0 @@ -{ - "name": "lite6", - "links": [ - { - "id": "base", - "parent": "world", - "translation": { - "x": 0, - "y": 0, - "z": 0 - } - }, - { - "id": "base_top", - "parent": "waist", - "translation": { - "x": 0, - "y": 0, - "z": 243.3 - }, - "geometry": { - "r": 40, - "l": 290, - "translation": { - "x": 0, - "y": 0, - "z": 145 - } - } - }, - { - "id": "upper_arm", - "parent": "shoulder", - "translation": { - "x": 0, - "y": 0, - "z": 200 - }, - "geometry": { - "r": 40, - "l": 280, - "translation": { - "x": 0, - "y": 105, - "z": 140 - } - } - }, - { - "id": "upper_forearm", - "parent": "elbow", - "translation": { - "x": 87, - "y": 0, - "z": 0 - }, - "geometry": { - "x": 165, - "y": 85, - "z": 85, - "translation": { - "x": 43.5, - "y": 0, - "z": 0 - } - } - }, - { - "id": "lower_forearm_filler", - "parent": "forearm_rot", - "translation": { - "x": 0, - "y": 0, - "z": 0 - }, - "geometry": { - "r": 40, - "l": 200, - "translation": { - "x": 0, - "y": 0, - "z": -30 - } - } - }, - { - "id": "lower_forearm", - "parent": "lower_forearm_filler", - "translation": { - "x": 0, - "y": 0, - "z": -227.6 - }, - "geometry": { - "x": 75, - "y": 35, - "z": 180, - "translation": { - "x": 0, - "y": 60, - "z": -175 - } - } - }, - { - "id": "wrist_link", - "parent": "wrist", - "translation": { - "x": 0, - "y": 0, - "z": -61.5 - }, - "geometry": { - "x": 75, - "y": 75, - "z": 100, - "translation": { - "x": 0, - "y": 0, - "z": -50 - } - } - }, - { - "id": "gripper_mount", - "parent": "gripper_rot", - "translation": { - "x": 0, - "y": 0, - "z": 0 - }, - "orientation": { - "type": "ov_degrees", - "value": { - "x": 0, - "y": 0, - "z": -1, - "th": 0 - } - } - } - ], - "joints": [ - { - "id": "waist", - "type": "revolute", - "parent": "base", - "axis": { - "x": 0, - "y": 0, - "z": 1 - }, - "max": 359, - "min": -359 - }, - { - "id": "shoulder", - "type": "revolute", - "parent": "base_top", - "axis": { - "x": 0, - "y": 1, - "z": 0 - }, - "max": 149, - "min": -149 - }, - { - "id": "elbow", - "type": "revolute", - "parent": "upper_arm", - "axis": { - "x": 0, - "y": -1, - "z": 0 - }, - "max": 299, - "min": -3 - }, - { - "id": "forearm_rot", - "type": "revolute", - "parent": "upper_forearm", - "axis": { - "x": 0, - "y": 0, - "z": -1 - }, - "max": 359, - "min": -359 - }, - { - "id": "wrist", - "type": "revolute", - "parent": "lower_forearm", - "axis": { - "x": 0, - "y": 1, - "z": 0 - }, - "max": 124, - "min": -124 - }, - { - "id": "gripper_rot", - "type": "revolute", - "parent": "wrist_link", - "axis": { - "x": 0, - "y": 0, - "z": -1 - }, - "max": 359, - "min": -359 - } - ] -} diff --git a/components/arm/xarm/xarm.go b/components/arm/xarm/xarm.go deleted file mode 100644 index cf7e050f585..00000000000 --- a/components/arm/xarm/xarm.go +++ /dev/null @@ -1,280 +0,0 @@ -// Package xarm implements some UFactory arms (xArm 6, xArm 7, and Lite 6). -package xarm - -import ( - "context" - // for embedding model file. - _ "embed" - "fmt" - "net" - "sync" - - "github.com/pkg/errors" - - "go.viam.com/rdk/components/arm" - "go.viam.com/rdk/logging" - "go.viam.com/rdk/operation" - "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/resource" - "go.viam.com/rdk/spatialmath" - "go.viam.com/rdk/utils" -) - -// Config is used for converting config attributes. -type Config struct { - Host string `json:"host"` - Port int `json:"port,omitempty"` - Speed float32 `json:"speed_degs_per_sec,omitempty"` - Acceleration float32 `json:"acceleration_degs_per_sec_per_sec,omitempty"` -} - -// Validate validates the config. -func (cfg *Config) Validate(path string) ([]string, error) { - if cfg.Host == "" { - return nil, resource.NewConfigValidationFieldRequiredError(path, "host") - } - return []string{}, nil -} - -const ( - defaultSpeed = 50. // degrees per second - defaultAccel = 100. // degrees per second per second - defaultPort = "502" - defaultMoveHz = 100. // Don't change this - - interwaypointAccel = 600. // degrees per second per second. All xarms max out at 1145 -) - -type xArm struct { - resource.Named - dof int - tid uint16 - moveHZ float64 // Number of joint positions to send per second - moveLock sync.Mutex - model referenceframe.Model - started bool - opMgr *operation.SingleOperationManager - logger logging.Logger - - mu sync.RWMutex - conn net.Conn - speed float64 // speed=max joint radians per second - acceleration float64 // acceleration= joint radians per second increase per second -} - -//go:embed xarm6_kinematics.json -var xArm6modeljson []byte - -//go:embed xarm7_kinematics.json -var xArm7modeljson []byte - -//go:embed lite6_kinematics.json -var lite6modeljson []byte - -const ( - ModelName6DOF = "xArm6" // ModelName6DOF is the name of a UFactory xArm 6 - ModelName7DOF = "xArm7" // ModelName7DOF is the name of a UFactory xArm 7 - ModelNameLite = "lite6" // ModelNameLite is the name of a UFactory Lite 6 -) - -// MakeModelFrame returns the kinematics model of the xarm arm, which has all Frame information. -func MakeModelFrame(name, modelName string) (referenceframe.Model, error) { - switch modelName { - case ModelName6DOF: - return referenceframe.UnmarshalModelJSON(xArm6modeljson, name) - case ModelNameLite: - return referenceframe.UnmarshalModelJSON(lite6modeljson, name) - case ModelName7DOF: - return referenceframe.UnmarshalModelJSON(xArm7modeljson, name) - default: - return nil, fmt.Errorf("no kinematics information for xarm of model %s", modelName) - } -} - -func init() { - for _, armModelName := range []string{ModelName6DOF, ModelName7DOF, ModelNameLite} { - localArmModelName := armModelName - armModel := resource.DefaultModelFamily.WithModel(armModelName) - resource.RegisterComponent(arm.API, armModel, resource.Registration[arm.Arm, *Config]{ - Constructor: func( - ctx context.Context, - _ resource.Dependencies, - conf resource.Config, - logger logging.Logger, - ) (arm.Arm, error) { - return NewxArm(ctx, conf, logger, localArmModelName) - }, - }) - } -} - -// NewxArm returns a new xArm of the specified modelName. -func NewxArm(ctx context.Context, conf resource.Config, logger logging.Logger, modelName string) (arm.Arm, error) { - model, err := MakeModelFrame(conf.Name, modelName) - if err != nil { - return nil, err - } - - xA := xArm{ - Named: conf.ResourceName().AsNamed(), - dof: len(model.DoF()), - tid: 0, - moveHZ: defaultMoveHz, - model: model, - started: false, - opMgr: operation.NewSingleOperationManager(), - logger: logger, - } - - if err := xA.Reconfigure(ctx, nil, conf); err != nil { - return nil, err - } - - return &xA, nil -} - -// Reconfigure atomically reconfigures this arm in place based on the new config. -func (x *xArm) Reconfigure(ctx context.Context, deps resource.Dependencies, conf resource.Config) error { - newConf, err := resource.NativeConfig[*Config](conf) - if err != nil { - return err - } - - if newConf.Host == "" { - return errors.New("xArm host not set") - } - - speed := newConf.Speed - if speed == 0 { - speed = defaultSpeed - } - - acceleration := newConf.Acceleration - if acceleration == 0 { - acceleration = defaultAccel - } - if acceleration < 0 { - return fmt.Errorf("given acceleration %f cannot be negative", acceleration) - } - - port := fmt.Sprintf("%d", newConf.Port) - if newConf.Port == 0 { - port = defaultPort - } - - x.mu.Lock() - defer x.mu.Unlock() - - newAddr := net.JoinHostPort(newConf.Host, port) - if x.conn == nil || x.conn.RemoteAddr().String() != newAddr { - // Need a new or replacement connection - var d net.Dialer - newConn, err := d.DialContext(ctx, "tcp", newAddr) - if err != nil { - return err - } - if x.conn != nil { - if err := x.conn.Close(); err != nil { - x.logger.CWarnw(ctx, "error closing old connection but will continue with reconfiguration", "error", err) - } - } - x.conn = newConn - - if err := x.start(ctx); err != nil { - return errors.Wrap(err, "failed to start on reconfigure") - } - } - - x.acceleration = utils.DegToRad(float64(acceleration)) - x.speed = utils.DegToRad(float64(speed)) - return nil -} - -func (x *xArm) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { - return x.JointPositions(ctx, nil) -} - -func (x *xArm) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Input) error { - return x.MoveThroughJointPositions(ctx, inputSteps, nil, nil) -} - -func (x *xArm) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { - inputs, err := x.CurrentInputs(ctx) - if err != nil { - return nil, err - } - gif, err := x.model.Geometries(inputs) - if err != nil { - return nil, err - } - return gif.Geometries(), nil -} - -// ModelFrame returns all the information necessary for including the arm in a FrameSystem. -func (x *xArm) ModelFrame() referenceframe.Model { - return x.model -} - -func (x *xArm) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) { - resp := map[string]interface{}{} - validCommand := false - - if _, ok := cmd["setup_gripper"]; ok { - if err := x.enableGripper(ctx); err != nil { - return nil, err - } - if err := x.setGripperMode(ctx, false); err != nil { - return nil, err - } - validCommand = true - } - if val, ok := cmd["move_gripper"]; ok { - position, ok := val.(float64) - if !ok || position < -10 || position > 850 { - return nil, fmt.Errorf("must move gripper to an int between 0 and 840 %v", val) - } - if err := x.setGripperPosition(ctx, uint32(position)); err != nil { - return nil, err - } - validCommand = true - } - if _, ok := cmd["load"]; ok { - loadInformation, err := x.getLoad(ctx) - if err != nil { - return nil, err - } - loadInformationInterface, ok := loadInformation["loads"] - if !ok { - return nil, errors.New("could not read loadInformation") - } - resp["load"] = loadInformationInterface - validCommand = true - } - if val, ok := cmd["set_speed"]; ok { - speed, err := utils.AssertType[float64](val) - if err != nil { - return nil, err - } - if speed <= 0 { - return nil, errors.New("speed cannot be less than or equal to zero") - } - x.speed = utils.DegToRad(speed) - validCommand = true - } - if val, ok := cmd["set_acceleration"]; ok { - acceleration, err := utils.AssertType[float64](val) - if err != nil { - return nil, err - } - if acceleration <= 0 { - return nil, errors.New("acceleration cannot be less than or equal to zero") - } - x.acceleration = utils.DegToRad(acceleration) - validCommand = true - } - - if !validCommand { - return nil, errors.New("command not found") - } - return resp, nil -} diff --git a/components/arm/xarm/xarm6_kinematics.json b/components/arm/xarm/xarm6_kinematics.json deleted file mode 100644 index cb4c8af962d..00000000000 --- a/components/arm/xarm/xarm6_kinematics.json +++ /dev/null @@ -1,217 +0,0 @@ -{ - "name": "xArm6", - "links": [ - { - "id": "base", - "parent": "world", - "translation": { - "x": 0, - "y": 0, - "z": 0 - } - }, - { - "id": "base_top", - "parent": "waist", - "translation": { - "x": 0, - "y": 0, - "z": 267 - }, - "geometry": { - "r": 50, - "l": 320, - "translation": { - "x": 0, - "y": 0, - "z": 160 - } - } - }, - { - "id": "upper_arm", - "parent": "shoulder", - "translation": { - "x": 53.5, - "y": 0, - "z": 284.5 - }, - "geometry": { - "x": 110, - "y": 190, - "z": 370, - "translation": { - "x": 0, - "y": 0, - "z": 135 - } - } - }, - { - "id": "upper_forearm", - "parent": "elbow", - "translation": { - "x": 77.5, - "y": 0, - "z": -172.5 - }, - "geometry": { - "x": 100, - "y": 190, - "z": 250, - "translation": { - "x": 49.49, - "y": 0, - "z": -49.49 - }, - "orientation": { - "type": "ov_degrees", - "value": { - "x": 0.707106, - "y": 0, - "z": -0.707106, - "th": 0 - } - } - } - }, - { - "id": "lower_forearm", - "parent": "forearm_rot", - "translation": { - "x": 0, - "y": 0, - "z": -170 - }, - "geometry": { - "r": 45, - "l": 285, - "translation": { - "x": 0, - "y": -27.5, - "z": -104.8 - }, - "orientation": { - "type": "ov_degrees", - "value": { - "th": -90, - "x": 0, - "y": 0.2537568, - "z": 0.9672615 - } - } - } - }, - { - "id": "wrist_link", - "parent": "wrist", - "translation": { - "x": 76, - "y": 0, - "z": -97 - }, - "geometry": { - "x": 150, - "y": 100, - "z": 135, - "translation": { - "x": 75, - "y": 10, - "z": -67.5 - } - } - }, - { - "id": "gripper_mount", - "parent": "gripper_rot", - "translation": { - "x": 0, - "y": 0, - "z": 0 - }, - "orientation": { - "type": "ov_degrees", - "value": { - "x": 0, - "y": 0, - "z": -1, - "th": 0 - } - } - } - ], - "joints": [ - { - "id": "waist", - "type": "revolute", - "parent": "base", - "axis": { - "x": 0, - "y": 0, - "z": 1 - }, - "max": 359, - "min": -359 - }, - { - "id": "shoulder", - "type": "revolute", - "parent": "base_top", - "axis": { - "x": 0, - "y": 1, - "z": 0 - }, - "max": 120, - "min": -118 - }, - { - "id": "elbow", - "type": "revolute", - "parent": "upper_arm", - "axis": { - "x": 0, - "y": 1, - "z": 0 - }, - "max": 10, - "min": -225 - }, - { - "id": "forearm_rot", - "type": "revolute", - "parent": "upper_forearm", - "axis": { - "x": 0, - "y": 0, - "z": -1 - }, - "max": 359, - "min": -359 - }, - { - "id": "wrist", - "type": "revolute", - "parent": "lower_forearm", - "axis": { - "x": 0, - "y": 1, - "z": 0 - }, - "max": 179, - "min": -97 - }, - { - "id": "gripper_rot", - "type": "revolute", - "parent": "wrist_link", - "axis": { - "x": 0, - "y": 0, - "z": -1 - }, - "max": 359, - "min": -359 - } - ] -} diff --git a/components/arm/xarm/xarm7_kinematics.json b/components/arm/xarm/xarm7_kinematics.json deleted file mode 100644 index a60da81ddd1..00000000000 --- a/components/arm/xarm/xarm7_kinematics.json +++ /dev/null @@ -1,257 +0,0 @@ -{ - "name": "xArm7", - "links": [ - { - "id": "base", - "parent": "world", - "translation": { - "x": 0, - "y": 0, - "z": 0 - } - }, - { - "id": "base_top", - "parent": "waist", - "translation": { - "x": 0, - "y": 0, - "z": 267 - }, - "geometry": { - "r": 50, - "l": 320, - "translation": { - "x": 0, - "y": 0, - "z": 160 - } - } - }, - { - "id": "base_arm_link", - "parent": "shoulder", - "translation": { - "x": 0, - "y": 0, - "z": 200 - }, - "geometry": { - "x": 110, - "y": 190, - "z": 220, - "translation": { - "x": 0, - "y": 30, - "z": 100 - } - } - }, - { - "id": "upper_arm", - "parent": "upper_arm_rot", - "translation": { - "x": 52.5, - "y": 0, - "z": 93 - }, - "geometry": { - "x": 100, - "y": 130, - "z": 180, - "translation": { - "x": 32.88, - "y": -10, - "z": 32.88 - }, - "orientation": { - "type": "ov_degrees", - "value": { - "x": 0.707106, - "y": 0, - "z": 0.707106, - "th": 0 - } - } - } - }, - { - "id": "upper_forearm", - "parent": "elbow", - "translation": { - "x": 77.5, - "y": 0, - "z": -172.5 - }, - "geometry": { - "x": 100, - "y": 150, - "z": 250, - "translation": { - "x": 60.98, - "y": -37.5, - "z": -60.98 - }, - "orientation": { - "type": "ov_degrees", - "value": { - "x": 0.707106, - "y": 0, - "z": -0.707106, - "th": 0 - } - } - } - }, - { - "id": "lower_forearm", - "parent": "forearm_rot", - "translation": { - "x": 0, - "y": 0, - "z": -170 - }, - "geometry": { - "r": 45, - "l": 285, - "translation": { - "x": 0, - "y": -27.5, - "z": -104.8 - }, - "orientation": { - "type": "ov_degrees", - "value": { - "th": -90, - "x": 0, - "y": 0.2537568, - "z": 0.9672615 - } - } - } - }, - { - "id": "wrist_link", - "parent": "wrist", - "translation": { - "x": 76, - "y": 0, - "z": -97 - }, - "geometry": { - "x": 150, - "y": 100, - "z": 135, - "translation": { - "x": 75, - "y": 0, - "z": -67.5 - } - } - }, - { - "id": "gripper_mount", - "parent": "gripper_rot", - "translation": { - "x": 0, - "y": 0, - "z": 0 - }, - "orientation": { - "type": "ov_degrees", - "value": { - "x": 0, - "y": 0, - "z": -1, - "th": 0 - } - } - } - ], - "joints": [ - { - "id": "waist", - "type": "revolute", - "parent": "base", - "axis": { - "x": 0, - "y": 0, - "z": 1 - }, - "max": 359, - "min": -359 - }, - { - "id": "shoulder", - "type": "revolute", - "parent": "base_top", - "axis": { - "x": 0, - "y": 1, - "z": 0 - }, - "max": 120, - "min": -117.5 - }, - { - "id": "upper_arm_rot", - "type": "revolute", - "parent": "base_arm_link", - "axis": { - "x": 0, - "y": 0, - "z": 1 - }, - "max": 359, - "min": -359 - }, - { - "id": "elbow", - "type": "revolute", - "parent": "upper_arm", - "axis": { - "x": 0, - "y": -1, - "z": 0 - }, - "max": 225, - "min": -11 - }, - { - "id": "forearm_rot", - "type": "revolute", - "parent": "upper_forearm", - "axis": { - "x": 0, - "y": 0, - "z": -1 - }, - "max": 359, - "min": -359 - }, - { - "id": "wrist", - "type": "revolute", - "parent": "lower_forearm", - "axis": { - "x": 0, - "y": 1, - "z": 0 - }, - "max": 179, - "min": -97 - }, - { - "id": "gripper_rot", - "type": "revolute", - "parent": "wrist_link", - "axis": { - "x": 0, - "y": 0, - "z": -1 - }, - "max": 359, - "min": -359 - } - ] -} diff --git a/components/arm/xarm/xarm_comm.go b/components/arm/xarm/xarm_comm.go deleted file mode 100644 index 50e5396817b..00000000000 --- a/components/arm/xarm/xarm_comm.go +++ /dev/null @@ -1,667 +0,0 @@ -package xarm - -import ( - "context" - "encoding/binary" - "errors" - "math" - "time" - - "go.uber.org/multierr" - "go.viam.com/utils" - - "go.viam.com/rdk/components/arm" - "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/services/motion" - "go.viam.com/rdk/spatialmath" - rutils "go.viam.com/rdk/utils" -) - -var servoErrorMap = map[byte]string{ - 0x00: "xArm Servo: Joint Communication Error", - 0x0A: "xArm Servo: Current Detection Error", - 0x0B: "xArm Servo: Joint Overcurrent", - 0x0C: "xArm Servo: Joint Overspeed", - 0x0E: "xArm Servo: Position Command Overlimit", - 0x0F: "xArm Servo: Joints Overheat", - 0x10: "xArm Servo: Encoder Initialization Error", - 0x11: "xArm Servo: Single-turn Encoder Error", - 0x12: "xArm Servo: Multi-turn Encoder Error", - 0x13: "xArm Servo: Low Battery Voltage", - 0x14: "xArm Servo: Driver IC Hardware Error", - 0x15: "xArm Servo: Driver IC Init Error", - 0x16: "xArm Servo: Encoder Config Error", - 0x17: "xArm Servo: Large Motor Position Deviation", - 0x1A: "xArm Servo: Joint N Positive Overrun", - 0x1B: "xArm Servo: Joint N Negative Overrun", - 0x1C: "xArm Servo: Joint Commands Error", - 0x21: "xArm Servo: Drive Overloaded", - 0x22: "xArm Servo: Motor Overload", - 0x23: "xArm Servo: Motor Type Error", - 0x24: "xArm Servo: Driver Type Error", - 0x27: "xArm Servo: Joint Overvoltage", - 0x28: "xArm Servo: Joint Undervoltage", - 0x31: "xArm Servo: EEPROM RW Error", - 0x34: "xArm Servo: Initialization of Motor Angle Error", -} - -var armBoxErrorMap = map[byte]string{ - 0x01: "xArm: Emergency Stop Button Pushed In", - 0x02: "xArm: Emergency IO Triggered", - 0x03: "xArm: Emergency Stop 3-State Switch Pressed", - 0x0B: "xArm: Power Cycle Required", - 0x0C: "xArm: Power Cycle Required", - 0x0D: "xArm: Power Cycle Required", - 0x0E: "xArm: Power Cycle Required", - 0x0F: "xArm: Power Cycle Required", - 0x10: "xArm: Power Cycle Required", - 0x11: "xArm: Power Cycle Required", - 0x13: "xArm: Gripper Communication Error", - 0x15: "xArm: Kinematic Error", - 0x16: "xArm: Self Collision Error", - 0x17: "xArm: Joint Angle Exceeds Limit", - 0x18: "xArm: Speed Exceeds Limit", - 0x19: "xArm: Planning Error", - 0x1A: "xArm: Linux RT Error", - 0x1B: "xArm: Command Reply Error", - 0x1C: "xArm: End Module Communication Error", - 0x1D: "xArm: Other Errors", - 0x1E: "xArm: Feedback Speed Exceeds Limit", - 0x1F: "xArm: Collision Caused Abnormal Current", - 0x20: "xArm: Three-point Drawing Circle Calculation Error", - 0x21: "xArm: Abnormal Arm Current", - 0x22: "xArm: Recording Timeout", - 0x23: "xArm: Safety Boundary Limit", - 0x24: "xArm: Delay Command Limit Exceeded", - 0x25: "xArm: Abnormal Motion in Manual Mode", - 0x26: "xArm: Abnormal Joint Angle", - 0x27: "xArm: Abnormal Communication Between Power Boards", -} - -var armBoxWarnMap = map[byte]string{ - 0x0B: "xArm Warning: Buffer Overflow", - 0x0C: "xArm Warning: Command Parameter Abnormal", - 0x0D: "xArm Warning: Unknown Command", - 0x0E: "xArm Warning: Command No Solution", -} - -var regMap = map[string]byte{ - "Version": 0x01, - "ActualCurrent": 0x05, - "Shutdown": 0x0A, - "ToggleServo": 0x0B, - "SetState": 0x0C, - "GetState": 0x0D, - "CmdCount": 0x0E, - "GetError": 0x0F, - "ClearError": 0x10, - "ClearWarn": 0x11, - "ToggleBrake": 0x12, - "SetMode": 0x13, - "MoveJoints": 0x1D, - "ZeroJoints": 0x19, - "JointPos": 0x2A, - "SetBound": 0x34, - "EnableBound": 0x34, - "CurrentTorque": 0x37, - "SetEEModel": 0x4E, - "ServoError": 0x6A, - "GripperControl": 0x7C, - "LoadID": 0xCC, -} - -type cmd struct { - tid uint16 - prot uint16 - reg byte - params []byte -} - -func (c *cmd) bytes() []byte { - var bin []byte - uintBin := make([]byte, 2) - binary.BigEndian.PutUint16(uintBin, c.tid) - bin = append(bin, uintBin...) - binary.BigEndian.PutUint16(uintBin, c.prot) - bin = append(bin, uintBin...) - binary.BigEndian.PutUint16(uintBin, 1+uint16(len(c.params))) - bin = append(bin, uintBin...) - bin = append(bin, c.reg) - bin = append(bin, c.params...) - return bin -} - -func (x *xArm) newCmd(reg byte) cmd { - x.tid++ - return cmd{tid: x.tid, prot: 2, reg: reg} -} - -func (x *xArm) send(ctx context.Context, c cmd, checkError bool) (cmd, error) { - x.moveLock.Lock() - b := c.bytes() - - // add deadline so we aren't waiting forever - if err := x.conn.SetDeadline(time.Now().Add(5 * time.Second)); err != nil { - return cmd{}, err - } - _, err := x.conn.Write(b) - if err != nil { - x.moveLock.Unlock() - return cmd{}, err - } - c2, err := x.response(ctx) - x.moveLock.Unlock() - if err != nil { - return cmd{}, err - } - if checkError { - state := c2.params[0] - if state&96 != 0 { - // Error (64) and/or warning (32) bit is set - e2 := multierr.Combine( - x.readError(ctx), - x.clearErrorAndWarning(ctx)) - return c2, e2 - } - // If bit 16 is set, that just means we have not yet activated motion- this happens at startup and shutdown - } - return c2, err -} - -func (x *xArm) response(ctx context.Context) (cmd, error) { - // Read response header - if x.conn == nil { - return cmd{}, errors.New("closed") - } - buf, err := utils.ReadBytes(ctx, x.conn, 7) - if err != nil { - return cmd{}, err - } - c := cmd{} - c.tid = binary.BigEndian.Uint16(buf[0:2]) - c.prot = binary.BigEndian.Uint16(buf[2:4]) - c.reg = buf[6] - length := binary.BigEndian.Uint16(buf[4:6]) - c.params, err = utils.ReadBytes(ctx, x.conn, int(length-1)) - if err != nil { - return cmd{}, err - } - return c, err -} - -// checkServoErrors will query the individual servos for any servo-specific -// errors. It may be useful for troubleshooting but as the SDK does not call -// it directly ever, we probably don't need to either during normal operation. -func (x *xArm) CheckServoErrors(ctx context.Context) error { - c := x.newCmd(regMap["ServoError"]) - e, err := x.send(ctx, c, false) - if err != nil { - return err - } - if len(e.params) < 18 { - return errors.New("bad servo error query response") - } - - // Get error codes for all (8) servos. - // xArm 6 has 6, xArm 7 has 7, and plus one in the xArm gripper - for i := 1; i < 9; i++ { - errCode := e.params[i*2] - errMsg, isErr := servoErrorMap[errCode] - if isErr { - err = multierr.Append(err, errors.New(errMsg)) - } - } - return err -} - -func (x *xArm) clearErrorAndWarning(ctx context.Context) error { - c1 := x.newCmd(regMap["ClearError"]) - c2 := x.newCmd(regMap["ClearWarn"]) - _, err1 := x.send(ctx, c1, false) - _, err2 := x.send(ctx, c2, false) - err3 := x.setMotionMode(ctx, 1) - err4 := x.setMotionState(ctx, 0) - return multierr.Combine(err1, err2, err3, err4) -} - -func (x *xArm) readError(ctx context.Context) error { - c := x.newCmd(regMap["GetError"]) - e, err := x.send(ctx, c, false) - if err != nil { - return err - } - if len(e.params) < 3 { - return errors.New("bad arm error query response") - } - - errCode := e.params[1] - warnCode := e.params[2] - errMsg, isErr := armBoxErrorMap[errCode] - warnMsg, isWarn := armBoxWarnMap[warnCode] - if isErr || isWarn { - return multierr.Combine(errors.New(errMsg), - errors.New(warnMsg)) - } - // Commands are returning error codes that are not mentioned in the - // developer manual - return errors.New("xArm: UNKNOWN ERROR") -} - -// setMotionState sets the motion state of the arm. -// Useful states: -// 0: Servo motion mode -// 3: Suspend current movement -// 4: Stop all motion, restart system -func (x *xArm) setMotionState(ctx context.Context, state byte) error { - c := x.newCmd(regMap["SetState"]) - c.params = append(c.params, state) - _, err := x.send(ctx, c, true) - return err -} - -// setMotionMode sets the motion mode of the arm. -// 0: Position Control Mode, i.e. "normal" mode -// 1: Servoj mode. This mode will immediately execute joint positions at the fastest available speed and is intended -// for streaming large numbers of joint positions to the arm. -// 2: Joint teaching mode, not useful right now -func (x *xArm) setMotionMode(ctx context.Context, state byte) error { - c := x.newCmd(regMap["SetMode"]) - c.params = append(c.params, state) - _, err := x.send(ctx, c, true) - return err -} - -// toggleServos toggles the servos on or off. -// True enables servos and disengages brakes. -// False disables servos without engaging brakes. -func (x *xArm) toggleServos(ctx context.Context, enable bool) error { - c := x.newCmd(regMap["ToggleServo"]) - var enByte byte - if enable { - enByte = 1 - } - c.params = append(c.params, 8, enByte) - _, err := x.send(ctx, c, true) - return err -} - -// toggleBrake toggles the brakes on or off. -// True disengages brakes, false engages them. -func (x *xArm) toggleBrake(ctx context.Context, disable bool) error { - c := x.newCmd(regMap["ToggleBrake"]) - var enByte byte - if disable { - enByte = 1 - } - c.params = append(c.params, 8, enByte) - _, err := x.send(ctx, c, true) - return err -} - -func (x *xArm) start(ctx context.Context) error { - err := x.toggleServos(ctx, true) - if err != nil { - return err - } - err = x.setMotionMode(ctx, 1) - if err != nil { - return err - } - if err := x.setMotionState(ctx, 0); err != nil { - return err - } - x.started = true - return nil -} - -// motionStopped will check if all arm pieces have stopped moving. -func (x *xArm) motionStopped(ctx context.Context) (bool, error) { - c := x.newCmd(regMap["GetState"]) - sData, err := x.send(ctx, c, true) - if err != nil { - return false, err - } - if len(sData.params) < 2 { - return false, errors.New("malformed state data response in motionStopped") - } - if sData.params[1] != 1 { - return true, nil - } - return false, nil -} - -// Close shuts down the arm servos and engages brakes. -func (x *xArm) Close(ctx context.Context) error { - if err := x.toggleBrake(ctx, false); err != nil { - return err - } - if err := x.toggleServos(ctx, false); err != nil { - return err - } - if err := x.setMotionState(ctx, 4); err != nil { - return err - } - x.mu.Lock() - defer x.mu.Unlock() - - if x.conn == nil { - return nil - } - err := x.conn.Close() - x.conn = nil - return err -} - -// MoveToJointPositions moves the arm to the requested joint positions. -func (x *xArm) MoveToJointPositions(ctx context.Context, newPositions []referenceframe.Input, extra map[string]interface{}) error { - ctx, done := x.opMgr.New(ctx) - defer done() - return x.GoToInputs(ctx, newPositions) -} - -func (x *xArm) MoveThroughJointPositions( - ctx context.Context, - positions [][]referenceframe.Input, - _ *arm.MoveOptions, - _ map[string]interface{}, -) error { - for _, goal := range positions { - // check that joint positions are not out of bounds - if err := arm.CheckDesiredJointPositions(ctx, x, goal); err != nil { - return err - } - } - curPos, err := x.JointPositions(ctx, nil) - if err != nil { - return err - } - armRawSteps, err := x.createRawJointSteps(curPos, positions) - if err != nil { - return err - } - return x.executeInputs(ctx, armRawSteps) -} - -// Using the configured moveHz, joint speed, and joint acceleration, create the series of joint positions for the arm to follow, -// using a trapezoidal velocity profile to blend between waypoints to the extent possible. -func (x *xArm) createRawJointSteps(startInputs []referenceframe.Input, inputSteps [][]referenceframe.Input) ([][]float64, error) { - x.mu.RLock() - speed := x.speed - acceleration := x.acceleration - moveHZ := x.moveHZ - x.mu.RUnlock() - // Generate list of joint positions to pass through - // This is almost-calculus but not quite because it's explicitly discretized - accelStep := acceleration / moveHZ - interwaypointAccelStep := interwaypointAccel / moveHZ - - from := referenceframe.InputsToFloats(startInputs) - - // We want smooth acceleration/motion but there's no guarantee the provided inputs have continuous velocity signs - floatMaxDiff := func(from, to []float64) float64 { - max := 0. - for i, toInput := range to { - diff := math.Abs(toInput - from[i]) - if diff > max { - max = diff - } - } - return max - } - - // Preprocess steps into step counts - stepTotal := 0. - displacementTotal := 0. - - for _, toInputs := range inputSteps { - to := referenceframe.InputsToFloats(toInputs) - max := floatMaxDiff(from, to) - displacementTotal += max - nSteps := (math.Abs(max) / speed) * moveHZ - stepTotal += nSteps - from = to - } - - nominalAccelSteps := int((speed / acceleration) * moveHZ) // This many steps to accelerate, and the same to decelerate - if float64(nominalAccelSteps) > stepTotal*0.95 { - nominalAccelSteps = int(0.95 * math.Sqrt(displacementTotal/acceleration) * moveHZ) - } - maxVel := (float64(nominalAccelSteps) / moveHZ) * acceleration - - inputStepsReversed := [][]referenceframe.Input{} - for i := len(inputSteps) - 1; i >= 0; i-- { - inputStepsReversed = append(inputStepsReversed, inputSteps[i]) - } - inputStepsReversed = append(inputStepsReversed, startInputs) - - accelCurve := func( - startInputs []referenceframe.Input, - allInputSteps [][]referenceframe.Input, - stopVel float64, - ) (int, [][]float64, error) { - currSpeed := accelStep - steps := [][]float64{} - from = referenceframe.InputsToFloats(startInputs) - lastInputs := startInputs - for i, toInputs := range allInputSteps { - to := referenceframe.InputsToFloats(toInputs) - runningFrom := from - - for currDiff := floatMaxDiff(runningFrom, to); currDiff > 1e-6; currDiff = floatMaxDiff(runningFrom, to) { - if currSpeed <= 0 { - break - } - nSteps := (currDiff / currSpeed) * moveHZ - stepSize := 1. - if nSteps <= 1 { - if currDiff == 0 { - break - } - stepSize = nSteps - } - nextInputs, err := x.model.Interpolate(lastInputs, toInputs, stepSize/nSteps) - if err != nil { - return 0, nil, err - } - runningFrom = referenceframe.InputsToFloats(nextInputs) - steps = append(steps, referenceframe.InputsToFloats(nextInputs)) - - if currSpeed < speed { - currSpeed += accelStep * stepSize - if currSpeed > speed { - currSpeed = speed - } - } else { - // If we reach max speed, accelerate at max for the remainder of the move - accelStep = interwaypointAccelStep - } - - if currSpeed >= stopVel-1e-6 { - return i, steps, nil - } - - lastInputs = nextInputs - } - lastInputs = toInputs - from = to - } - return len(allInputSteps), steps, nil - } - - decelStart, decelSteps, err := accelCurve(inputStepsReversed[0], inputStepsReversed, maxVel) - if err != nil { - return nil, err - } - accelStop := len(inputSteps) - decelStart - accelInputSteps := [][]referenceframe.Input{} - for i, inputStep := range inputSteps { - if i == accelStop { - accelInputSteps = append(accelInputSteps, referenceframe.FloatsToInputs(decelSteps[len(decelSteps)-1])) - break - } - accelInputSteps = append(accelInputSteps, inputStep) - } - _, accelSteps, err := accelCurve(startInputs, accelInputSteps, math.Inf(1)) - if err != nil { - return nil, err - } - for i := len(decelSteps) - 2; i >= 0; i-- { - accelSteps = append(accelSteps, decelSteps[i]) - } - - return accelSteps, nil -} - -func (x *xArm) executeInputs(ctx context.Context, rawSteps [][]float64) error { - if !x.started { - if err := x.start(ctx); err != nil { - return err - } - } - - // convenience for structuring and sending individual joint steps - for _, step := range rawSteps { - c := x.newCmd(regMap["MoveJoints"]) - jFloatBytes := make([]byte, 4) - for _, jRad := range step { - binary.LittleEndian.PutUint32(jFloatBytes, math.Float32bits(float32(jRad))) - c.params = append(c.params, jFloatBytes...) - } - // xarm 6 has 6 joints, but protocol needs 7- add 4 bytes for a blank 7th joint - for dof := x.dof; dof < 7; dof++ { - c.params = append(c.params, 0, 0, 0, 0) - } - // When in servoj mode, motion time, speed, and acceleration are not handled by the control box - c.params = append(c.params, 0, 0, 0, 0) - c.params = append(c.params, 0, 0, 0, 0) - c.params = append(c.params, 0, 0, 0, 0) - _, err := x.send(ctx, c, true) - if err != nil { - return err - } - if !utils.SelectContextOrWait(ctx, time.Duration(1000000./x.moveHZ)*time.Microsecond) { - return ctx.Err() - } - } - - return nil -} - -// EndPosition computes and returns the current cartesian position. -func (x *xArm) EndPosition(ctx context.Context, extra map[string]interface{}) (spatialmath.Pose, error) { - joints, err := x.CurrentInputs(ctx) - if err != nil { - return nil, err - } - return referenceframe.ComputeOOBPosition(x.model, joints) -} - -// MoveToPosition moves the arm to the specified cartesian position. -func (x *xArm) MoveToPosition(ctx context.Context, pos spatialmath.Pose, extra map[string]interface{}) error { - ctx, done := x.opMgr.New(ctx) - defer done() - if !x.started { - if err := x.start(ctx); err != nil { - return err - } - } - if err := motion.MoveArm(ctx, x.logger, x, pos); err != nil { - return err - } - return x.opMgr.WaitForSuccess( - ctx, - time.Millisecond*50, - x.motionStopped, - ) -} - -// JointPositions returns the current positions of all joints. -func (x *xArm) JointPositions(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) { - c := x.newCmd(regMap["JointPos"]) - - jData, err := x.send(ctx, c, true) - if err != nil { - return nil, err - } - var radians []float64 - for i := 0; i < x.dof; i++ { - idx := i*4 + 1 - radians = append(radians, float64(rutils.Float32FromBytesLE((jData.params[idx : idx+4])))) - } - return referenceframe.FloatsToInputs(radians), nil -} - -// Stop stops the xArm but also reinitializes the arm so it can take commands again. -func (x *xArm) Stop(ctx context.Context, extra map[string]interface{}) error { - ctx, done := x.opMgr.New(ctx) - defer done() - x.started = false - if err := x.setMotionState(ctx, 3); err != nil { - return err - } - return x.start(ctx) -} - -// IsMoving returns whether the arm is moving. -func (x *xArm) IsMoving(ctx context.Context) (bool, error) { - return x.opMgr.OpRunning(), nil -} - -func (x *xArm) enableGripper(ctx context.Context) error { - c := x.newCmd(regMap["GripperControl"]) - c.params = append(c.params, 0x09) - c.params = append(c.params, 0x08) - c.params = append(c.params, 0x10) - c.params = append(c.params, 0x01, 0x00) - c.params = append(c.params, 0x00, 0x01) - c.params = append(c.params, 0x02) - c.params = append(c.params, 0x00, 0x01) - _, err := x.send(ctx, c, true) - return err -} - -func (x *xArm) setGripperMode(ctx context.Context, speed bool) error { - c := x.newCmd(regMap["GripperControl"]) - c.params = append(c.params, 0x09) - c.params = append(c.params, 0x08) - c.params = append(c.params, 0x10) - c.params = append(c.params, 0x01, 0x01) - c.params = append(c.params, 0x00, 0x01) - c.params = append(c.params, 0x02) - if speed { - c.params = append(c.params, 0x00, 0x01) - } else { - c.params = append(c.params, 0x00, 0x00) - } - _, err := x.send(ctx, c, true) - return err -} - -func (x *xArm) setGripperPosition(ctx context.Context, position uint32) error { - c := x.newCmd(regMap["GripperControl"]) - c.params = append(c.params, 0x09) - c.params = append(c.params, 0x08) - c.params = append(c.params, 0x10) - c.params = append(c.params, 0x07, 0x00) - c.params = append(c.params, 0x00, 0x02) - c.params = append(c.params, 0x04) - tmpBytes := make([]byte, 4) - binary.BigEndian.PutUint32(tmpBytes, position) - x.logger.Info("tmpBytes", tmpBytes) - c.params = append(c.params, tmpBytes...) - _, err := x.send(ctx, c, true) - return err -} - -func (x *xArm) getLoad(ctx context.Context) (map[string]interface{}, error) { - c := x.newCmd(regMap["CurrentTorque"]) - // ~ c.params = append(c.params, 0x01) - loadData, err := x.send(ctx, c, true) - var loads []float64 - for i := 0; i < x.dof; i++ { - idx := i*4 + 1 - loads = append(loads, float64(rutils.Float32FromBytesLE((loadData.params[idx : idx+4])))) - } - - return map[string]interface{}{"load": loads}, err -} diff --git a/components/arm/xarm/xarm_test.go b/components/arm/xarm/xarm_test.go deleted file mode 100644 index 21f11139fc9..00000000000 --- a/components/arm/xarm/xarm_test.go +++ /dev/null @@ -1,205 +0,0 @@ -package xarm - -import ( - "context" - "net" - "strconv" - "testing" - - "github.com/golang/geo/r3" - pb "go.viam.com/api/common/v1" - "go.viam.com/test" - - "go.viam.com/rdk/logging" - "go.viam.com/rdk/motionplan" - frame "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/resource" - spatial "go.viam.com/rdk/spatialmath" - "go.viam.com/rdk/utils" -) - -var ( - home7 = frame.FloatsToInputs([]float64{0, 0, 0, 0, 0, 0, 0}) - wbY = -426. -) - -// This will test solving the path to write the word "VIAM" on a whiteboard. -func TestWriteViam(t *testing.T) { - fs := frame.NewEmptyFrameSystem("test") - - ctx := context.Background() - logger := logging.NewTestLogger(t) - m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm7_kinematics.json"), "") - test.That(t, err, test.ShouldBeNil) - - err = fs.AddFrame(m, fs.World()) - test.That(t, err, test.ShouldBeNil) - - markerOriginFrame, err := frame.NewStaticFrame( - "marker_origin", - spatial.NewPoseFromOrientation(&spatial.OrientationVectorDegrees{OY: -1, OZ: 1}), - ) - test.That(t, err, test.ShouldBeNil) - markerFrame, err := frame.NewStaticFrame("marker", spatial.NewPoseFromPoint(r3.Vector{0, 0, 160})) - test.That(t, err, test.ShouldBeNil) - err = fs.AddFrame(markerOriginFrame, m) - test.That(t, err, test.ShouldBeNil) - err = fs.AddFrame(markerFrame, markerOriginFrame) - test.That(t, err, test.ShouldBeNil) - - eraserOriginFrame, err := frame.NewStaticFrame( - "eraser_origin", - spatial.NewPoseFromOrientation(&spatial.OrientationVectorDegrees{OY: 1, OZ: 1}), - ) - test.That(t, err, test.ShouldBeNil) - eraserFrame, err := frame.NewStaticFrame("eraser", spatial.NewPoseFromPoint(r3.Vector{0, 0, 160})) - test.That(t, err, test.ShouldBeNil) - err = fs.AddFrame(eraserOriginFrame, m) - test.That(t, err, test.ShouldBeNil) - err = fs.AddFrame(eraserFrame, eraserOriginFrame) - test.That(t, err, test.ShouldBeNil) - - moveFrame := eraserFrame - - // draw pos start - goal := spatial.NewPoseFromProtobuf(&pb.Pose{ - X: 230, - Y: wbY + 10, - Z: 600, - OY: -1, - }) - - seedMap := map[string][]frame.Input{} - - seedMap[m.Name()] = home7 - - plan, err := motionplan.PlanMotion(ctx, &motionplan.PlanRequest{ - Logger: logger, - Goal: frame.NewPoseInFrame(frame.World, goal), - Frame: moveFrame, - StartConfiguration: seedMap, - FrameSystem: fs, - }) - test.That(t, err, test.ShouldBeNil) - - opt := map[string]interface{}{"motion_profile": motionplan.LinearMotionProfile} - goToGoal := func(seedMap map[string][]frame.Input, goal spatial.Pose) map[string][]frame.Input { - plan, err := motionplan.PlanMotion(ctx, &motionplan.PlanRequest{ - Logger: logger, - Goal: frame.NewPoseInFrame(fs.World().Name(), goal), - Frame: moveFrame, - StartConfiguration: seedMap, - FrameSystem: fs, - Options: opt, - }) - test.That(t, err, test.ShouldBeNil) - return plan.Trajectory()[len(plan.Trajectory())-1] - } - - seed := plan.Trajectory()[len(plan.Trajectory())-1] - for _, goal = range viamPoints { - seed = goToGoal(seed, goal) - } -} - -var viamPoints = []spatial.Pose{ - spatial.NewPoseFromProtobuf(&pb.Pose{X: 200, Y: wbY + 1.5, Z: 595, OY: -1}), - spatial.NewPoseFromProtobuf(&pb.Pose{X: 120, Y: wbY + 1.5, Z: 595, OY: -1}), -} - -func TestReconfigure(t *testing.T) { - listener1, err := net.Listen("tcp4", "127.0.0.1:0") - test.That(t, err, test.ShouldBeNil) - defer listener1.Close() - addr1 := listener1.Addr().String() - listener2, err := net.Listen("tcp4", "127.0.0.1:0") - test.That(t, err, test.ShouldBeNil) - defer listener2.Close() - addr2 := listener2.Addr().String() - host1, port1Str, err := net.SplitHostPort(addr1) - test.That(t, err, test.ShouldBeNil) - host2, port2Str, err := net.SplitHostPort(addr2) - test.That(t, err, test.ShouldBeNil) - - port1, err := strconv.ParseInt(port1Str, 10, 32) - test.That(t, err, test.ShouldBeNil) - port2, err := strconv.ParseInt(port2Str, 10, 32) - test.That(t, err, test.ShouldBeNil) - - cfg := resource.Config{ - Name: "testarm", - ConvertedAttributes: &Config{ - Speed: 0.3, - Host: host1, - Port: int(port1), - Acceleration: 0.1, - }, - } - - shouldNotReconnectCfg := resource.Config{ - Name: "testarm", - ConvertedAttributes: &Config{ - Speed: 0.5, - Host: host1, - Port: int(port1), - Acceleration: 0.3, - }, - } - - shouldReconnectCfg := resource.Config{ - Name: "testarm", - ConvertedAttributes: &Config{ - Speed: 0.6, - Host: host2, - Port: int(port2), - Acceleration: 0.34, - }, - } - - conf, err := resource.NativeConfig[*Config](cfg) - test.That(t, err, test.ShouldBeNil) - confNotReconnect, ok := shouldNotReconnectCfg.ConvertedAttributes.(*Config) - test.That(t, ok, test.ShouldBeTrue) - - conn1, err := net.Dial("tcp", listener1.Addr().String()) - test.That(t, err, test.ShouldBeNil) - xArm := &xArm{ - speed: utils.DegToRad(float64(conf.Speed)), - logger: logging.NewTestLogger(t), - } - xArm.mu.Lock() - xArm.conn = conn1 - xArm.mu.Unlock() - - ctx := context.Background() - - // scenario where we do no nothing - prevSpeed := xArm.speed - test.That(t, xArm.Reconfigure(ctx, nil, cfg), test.ShouldBeNil) - - xArm.mu.Lock() - currentConn := xArm.conn - xArm.mu.Unlock() - test.That(t, currentConn, test.ShouldEqual, conn1) - test.That(t, xArm.speed, test.ShouldEqual, prevSpeed) - - // scenario where we do not reconnect - test.That(t, xArm.Reconfigure(ctx, nil, shouldNotReconnectCfg), test.ShouldBeNil) - - xArm.mu.Lock() - currentConn = xArm.conn - xArm.mu.Unlock() - test.That(t, currentConn, test.ShouldEqual, conn1) - test.That(t, xArm.speed, test.ShouldEqual, float32(utils.DegToRad(float64(confNotReconnect.Speed)))) - - // scenario where we have to reconnect - err = xArm.Reconfigure(ctx, nil, shouldReconnectCfg) - test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldContainSubstring, "failed to start") - - xArm.mu.Lock() - currentConn = xArm.conn - xArm.mu.Unlock() - test.That(t, currentConn, test.ShouldNotEqual, conn1) - test.That(t, xArm.speed, test.ShouldEqual, float32(utils.DegToRad(float64(confNotReconnect.Speed)))) -} diff --git a/go.mod b/go.mod index 706a647dfe6..b576dc9b40d 100644 --- a/go.mod +++ b/go.mod @@ -155,6 +155,7 @@ require ( github.com/bufbuild/protocompile v0.5.1 // indirect github.com/butuzov/ireturn v0.3.0 // indirect github.com/butuzov/mirror v1.2.0 // indirect + github.com/bytedance/sonic/loader v0.2.1 // indirect github.com/campoy/embedmd v1.0.0 // indirect github.com/catenacyber/perfsprint v0.7.1 // indirect github.com/catppuccin/go v0.2.0 // indirect @@ -174,6 +175,7 @@ require ( github.com/chewxy/hm v1.0.0 // indirect github.com/chewxy/math32 v1.0.8 // indirect github.com/ckaznocha/intrange v0.2.0 // indirect + github.com/cloudwego/base64x v0.1.4 // indirect github.com/cncf/xds/go v0.0.0-20240423153145-555b57ec207b // indirect github.com/containerd/stargz-snapshotter/estargz v0.14.3 // indirect github.com/cpuguy83/go-md2man/v2 v2.0.4 // indirect @@ -432,6 +434,7 @@ require ( github.com/erikstmartin/go-testdb v0.0.0-20160219214506-8d10e4a1bae5 // indirect github.com/kylelemons/go-gypsy v1.0.0 // indirect github.com/pkg/errors v0.9.1 + github.com/viam-modules/viam-ufactory-xarm v0.0.0-20241121162723-0285e0118688 github.com/ziutek/mymysql v1.5.4 // indirect golang.org/x/exp v0.0.0-20240904232852-e7e105dedf7e ) diff --git a/go.sum b/go.sum index 5452c89ce1e..00206afe0c5 100644 --- a/go.sum +++ b/go.sum @@ -207,8 +207,11 @@ github.com/butuzov/ireturn v0.3.0 h1:hTjMqWw3y5JC3kpnC5vXmFJAWI/m31jaCYQqzkS6PL0 github.com/butuzov/ireturn v0.3.0/go.mod h1:A09nIiwiqzN/IoVo9ogpa0Hzi9fex1kd9PSD6edP5ZA= github.com/butuzov/mirror v1.2.0 h1:9YVK1qIjNspaqWutSv8gsge2e/Xpq1eqEkslEUHy5cs= github.com/butuzov/mirror v1.2.0/go.mod h1:DqZZDtzm42wIAIyHXeN8W/qb1EPlb9Qn/if9icBOpdQ= -github.com/bytedance/sonic v1.9.1 h1:6iJ6NqdoxCDr6mbY8h18oSO+cShGSMRGCEo7F2h0x8s= -github.com/bytedance/sonic v1.9.1/go.mod h1:i736AoUSYt75HyZLoJW9ERYxcy6eaN6h4BZXU064P/U= +github.com/bytedance/sonic v1.12.2 h1:oaMFuRTpMHYLpCntGca65YWt5ny+wAceDERTkT2L9lg= +github.com/bytedance/sonic v1.12.2/go.mod h1:B8Gt/XvtZ3Fqj+iSKMypzymZxw/FVwgIGKzMzT9r/rk= +github.com/bytedance/sonic/loader v0.1.1/go.mod h1:ncP89zfokxS5LZrJxl5z0UJcsk4M4yY2JpfqGeCtNLU= +github.com/bytedance/sonic/loader v0.2.1 h1:1GgorWTqf12TA8mma4DDSbaQigE2wOgQo7iCjjJv3+E= +github.com/bytedance/sonic/loader v0.2.1/go.mod h1:ncP89zfokxS5LZrJxl5z0UJcsk4M4yY2JpfqGeCtNLU= github.com/campoy/embedmd v1.0.0 h1:V4kI2qTJJLf4J29RzI/MAt2c3Bl4dQSYPuflzwFH2hY= github.com/campoy/embedmd v1.0.0/go.mod h1:oxyr9RCiSXg0M3VJ3ks0UGfp98BpSSGr0kpiX3MzVl8= github.com/casbin/casbin/v2 v2.1.2/go.mod h1:YcPU1XXisHhLzuxH9coDNf2FbKpjGlbCg3n9yuLkIJQ= @@ -252,8 +255,6 @@ github.com/charmbracelet/x/term v0.2.0 h1:cNB9Ot9q8I711MyZ7myUR5HFWL/lc3OpU8jZ4h github.com/charmbracelet/x/term v0.2.0/go.mod h1:GVxgxAbjUrmpvIINHIQnJJKpMlHiZ4cktEQCN6GWyF0= github.com/chavacava/garif v0.1.0 h1:2JHa3hbYf5D9dsgseMKAmc/MZ109otzgNFk5s87H9Pc= github.com/chavacava/garif v0.1.0/go.mod h1:XMyYCkEL58DF0oyW4qDjjnPWONs2HBqYKI+UIPD+Gww= -github.com/chenzhuoyu/base64x v0.0.0-20221115062448-fe3a3abad311 h1:qSGYFH7+jGhDF8vLC+iwCD4WpbV1EBDSzWkJODFLams= -github.com/chenzhuoyu/base64x v0.0.0-20221115062448-fe3a3abad311/go.mod h1:b583jCggY9gE99b6G5LEC39OIiVsWj+R97kbl5odCEk= github.com/chewxy/hm v1.0.0 h1:zy/TSv3LV2nD3dwUEQL2VhXeoXbb9QkpmdRAVUFiA6k= github.com/chewxy/hm v1.0.0/go.mod h1:qg9YI4q6Fkj/whwHR1D+bOGeF7SniIP40VweVepLjg0= github.com/chewxy/math32 v1.0.0/go.mod h1:Miac6hA1ohdDUTagnvJy/q+aNnEk16qWUdb8ZVhvCN0= @@ -266,6 +267,10 @@ github.com/ckaznocha/intrange v0.2.0 h1:FykcZuJ8BD7oX93YbO1UY9oZtkRbp+1/kJcDjkef github.com/ckaznocha/intrange v0.2.0/go.mod h1:r5I7nUlAAG56xmkOpw4XVr16BXhwYTUdcuRFeevn1oE= github.com/clbanning/x2j v0.0.0-20191024224557-825249438eec/go.mod h1:jMjuTZXRI4dUb/I5gc9Hdhagfvm9+RyrPryS/auMzxE= github.com/client9/misspell v0.3.4/go.mod h1:qj6jICC3Q7zFZvVWo7KLAzC3yx5G7kyvSDkc90ppPyw= +github.com/cloudwego/base64x v0.1.4 h1:jwCgWpFanWmN8xoIUHa2rtzmkd5J2plF/dnLS6Xd/0Y= +github.com/cloudwego/base64x v0.1.4/go.mod h1:0zlkT4Wn5C6NdauXdJRhSKRlJvmclQ1hhJgA0rcu/8w= +github.com/cloudwego/iasm v0.2.0 h1:1KNIy1I1H9hNNFEEH3DVnI4UujN+1zjpuk6gwHLTssg= +github.com/cloudwego/iasm v0.2.0/go.mod h1:8rXZaNYT2n95jn+zTI1sDr+IgcD2GVs0nlbbQPiEFhY= github.com/cncf/udpa/go v0.0.0-20191209042840-269d4d468f6f/go.mod h1:M8M6+tZqaGXZJjfX53e64911xZQV5JYwmTeXPW+k8Sc= github.com/cncf/udpa/go v0.0.0-20201120205902-5459f2c99403/go.mod h1:WmhPx2Nbnhtbo57+VJT5O0JRkEi1Wbu0z5j0R8u5Hbk= github.com/cncf/udpa/go v0.0.0-20210930031921-04548b0d99d4/go.mod h1:6pvJx4me5XPnfI9Z40ddWsdw2W/uZgQLFXToKeRcDiI= @@ -822,10 +827,12 @@ github.com/klauspost/compress v1.16.5 h1:IFV2oUNUzZaz+XyusxpLzpzS8Pt5rh0Z16For/d github.com/klauspost/compress v1.16.5/go.mod h1:ntbaceVETuRiXiv4DpjP66DpAtAGkEQskQzEyD//IeE= github.com/klauspost/cpuid v0.0.0-20180405133222-e7e905edc00e h1:+lIPJOWl+jSiJOc70QXJ07+2eg2Jy2EC7Mi11BWujeM= github.com/klauspost/cpuid v0.0.0-20180405133222-e7e905edc00e/go.mod h1:Pj4uuM528wm8OyEC2QMXAi2YiTZ96dNQPGgoMS4s3ek= +github.com/klauspost/cpuid/v2 v2.0.9/go.mod h1:FInQzS24/EEf25PyTYn52gqo7WaD8xa0213Md/qVLRg= github.com/klauspost/cpuid/v2 v2.2.4 h1:acbojRNwl3o09bUq+yDCtZFc1aiwaAAxtcn8YkZXnvk= github.com/klauspost/cpuid/v2 v2.2.4/go.mod h1:RVVoqg1df56z8g3pUjL/3lE5UfnlrJX8tyFgg4nqhuY= github.com/klauspost/pgzip v1.2.6 h1:8RXeL5crjEUFnR2/Sn6GJNWtSQ3Dk8pq4CL3jvdDyjU= github.com/klauspost/pgzip v1.2.6/go.mod h1:Ch1tH69qFZu15pkjo5kYi6mth2Zzwzt50oCQKQE9RUs= +github.com/knz/go-libedit v1.10.1/go.mod h1:MZTVkCWyz0oBc7JOWP3wNAzd002ZbM/5hgShxwh4x8M= github.com/konsorten/go-windows-terminal-sequences v1.0.1/go.mod h1:T0+1ngSBFLxvqU3pZ+m/2kptfBszLMUkC4ZK/EgS/cQ= github.com/konsorten/go-windows-terminal-sequences v1.0.3/go.mod h1:T0+1ngSBFLxvqU3pZ+m/2kptfBszLMUkC4ZK/EgS/cQ= github.com/kr/fs v0.1.0/go.mod h1:FFnZGqtBN9Gxj7eW1uZ42v5BccTP0vu6NEaFoC2HwRg= @@ -1394,6 +1401,8 @@ github.com/valyala/quicktemplate v1.6.3/go.mod h1:fwPzK2fHuYEODzJ9pkw0ipCPNHZ2tD github.com/valyala/tcplisten v0.0.0-20161114210144-ceec8f93295a/go.mod h1:v3UYOV9WzVtRmSR+PDvWpU/qWl4Wa5LApYYX4ZtKbio= github.com/vbatts/tar-split v0.11.3 h1:hLFqsOLQ1SsppQNTMpkpPXClLDfC2A3Zgy9OUU+RVck= github.com/vbatts/tar-split v0.11.3/go.mod h1:9QlHN18E+fEH7RdG+QAJJcuya3rqT7eXSTY7wGrAokY= +github.com/viam-modules/viam-ufactory-xarm v0.0.0-20241121162723-0285e0118688 h1:bzAd7S6+Ln0qyD2H69oVYLhA6oCNHYiyvX2+m12SU6w= +github.com/viam-modules/viam-ufactory-xarm v0.0.0-20241121162723-0285e0118688/go.mod h1:nd/mMK59zmB2tNAGreT9ge68MQYB+FU0xYmo+YBvpGg= github.com/viamrobotics/evdev v0.1.3 h1:mR4HFafvbc5Wx4Vp1AUJp6/aITfVx9AKyXWx+rWjpfc= github.com/viamrobotics/evdev v0.1.3/go.mod h1:N6nuZmPz7HEIpM7esNWwLxbYzqWqLSZkfI/1Sccckqk= github.com/viamrobotics/webrtc/v3 v3.99.10 h1:ykE14wm+HkqMD5Ozq4rvhzzfvnXAu14ak/HzA1OCzfY= @@ -2097,6 +2106,7 @@ mvdan.cc/unparam v0.0.0-20240528143540-8a5130ca722f/go.mod h1:RSLa7mKKCNeTTMHBw5 nhooyr.io/websocket v1.8.6/go.mod h1:B70DZP8IakI65RVQ51MsWP/8jndNma26DVA/nFSCgW0= nhooyr.io/websocket v1.8.7 h1:usjR2uOr/zjjkVMy0lW+PPohFok7PCow5sDjLgX4P4g= nhooyr.io/websocket v1.8.7/go.mod h1:B70DZP8IakI65RVQ51MsWP/8jndNma26DVA/nFSCgW0= +nullprogram.com/x/optparse v1.0.0/go.mod h1:KdyPE+Igbe0jQUrVfMqDMeJQIJZEuyV7pjYmp6pbG50= periph.io/x/conn/v3 v3.7.0 h1:f1EXLn4pkf7AEWwkol2gilCNZ0ElY+bxS4WE2PQXfrA= periph.io/x/conn/v3 v3.7.0/go.mod h1:ypY7UVxgDbP9PJGwFSVelRRagxyXYfttVh7hJZUHEhg= periph.io/x/host/v3 v3.8.1-0.20230331112814-9f0d9f7d76db h1:8+HL7DJFofYRhGoK/UdwhzvQj3I2HrKLQ6dkOC66CZY= From 601c88b665ffbdc5ac774838e0209c8b2cb4d494 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 21 Nov 2024 13:34:38 -0500 Subject: [PATCH 02/13] appease linter --- components/arm/fake/fake.go | 2 +- components/arm/register/register.go | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/components/arm/fake/fake.go b/components/arm/fake/fake.go index 58aef76158d..b862be410a1 100644 --- a/components/arm/fake/fake.go +++ b/components/arm/fake/fake.go @@ -8,8 +8,8 @@ import ( "sync" "github.com/pkg/errors" - xarm "github.com/viam-modules/viam-ufactory-xarm/arm" + "go.viam.com/rdk/components/arm" "go.viam.com/rdk/components/arm/eva" ur "go.viam.com/rdk/components/arm/universalrobots" diff --git a/components/arm/register/register.go b/components/arm/register/register.go index 1b58a7332df..6c079ffbbe3 100644 --- a/components/arm/register/register.go +++ b/components/arm/register/register.go @@ -4,6 +4,7 @@ package register import ( // register arms. _ "github.com/viam-modules/viam-ufactory-xarm/arm" + _ "go.viam.com/rdk/components/arm/eva" _ "go.viam.com/rdk/components/arm/fake" _ "go.viam.com/rdk/components/arm/universalrobots" From d13d58a0ab54455609d0f642307ba7ce2c82de20 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 21 Nov 2024 14:05:06 -0500 Subject: [PATCH 03/13] remove xArm from being registered within RDK --- components/arm/register/register.go | 2 -- 1 file changed, 2 deletions(-) diff --git a/components/arm/register/register.go b/components/arm/register/register.go index 6c079ffbbe3..a566fca2b63 100644 --- a/components/arm/register/register.go +++ b/components/arm/register/register.go @@ -3,8 +3,6 @@ package register import ( // register arms. - _ "github.com/viam-modules/viam-ufactory-xarm/arm" - _ "go.viam.com/rdk/components/arm/eva" _ "go.viam.com/rdk/components/arm/fake" _ "go.viam.com/rdk/components/arm/universalrobots" From 7031d82550e64a6b2f7a92dfd404cb0278398b14 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 21 Nov 2024 17:26:40 -0500 Subject: [PATCH 04/13] switch paths for building xArm6&7 from kinematics files --- components/arm/wrapper/wrapper_test.go | 4 +- motionplan/cBiRRT_test.go | 2 +- motionplan/collision_test.go | 4 +- motionplan/constraint_test.go | 6 +- motionplan/ik/nloptInverseKinematics_test.go | 2 +- motionplan/ik/solver_test.go | 4 +- motionplan/kinematic_test.go | 10 +- motionplan/motionPlanner_test.go | 10 +- referenceframe/model_json_test.go | 4 +- referenceframe/model_test.go | 8 +- .../motion/data/xarm6_kinematics_test.json | 217 +++++++++++++++ .../motion/data/xarm7_kinematics_test.json | 257 ++++++++++++++++++ 12 files changed, 501 insertions(+), 27 deletions(-) create mode 100644 services/motion/data/xarm6_kinematics_test.json create mode 100644 services/motion/data/xarm7_kinematics_test.json diff --git a/components/arm/wrapper/wrapper_test.go b/components/arm/wrapper/wrapper_test.go index b0d9bbd11af..859f00aaae1 100644 --- a/components/arm/wrapper/wrapper_test.go +++ b/components/arm/wrapper/wrapper_test.go @@ -27,7 +27,7 @@ func TestReconfigure(t *testing.T) { cfg1 := resource.Config{ Name: "testArm", ConvertedAttributes: &Config{ - ModelFilePath: "../xarm/xarm6_kinematics.json", + ModelFilePath: "../../../services/motion/data/xarm6_kinematics_test.json", ArmName: armName.ShortName(), }, } @@ -35,7 +35,7 @@ func TestReconfigure(t *testing.T) { cfg1Err := resource.Config{ Name: "testArm", ConvertedAttributes: &Config{ - ModelFilePath: "../xarm/xarm6_kinematics.json", + ModelFilePath: "../../../services/motion/data/xarm6_kinematics_test.json", ArmName: "dne1", }, } diff --git a/motionplan/cBiRRT_test.go b/motionplan/cBiRRT_test.go index 5378471cf47..66fa579f6c9 100644 --- a/motionplan/cBiRRT_test.go +++ b/motionplan/cBiRRT_test.go @@ -33,7 +33,7 @@ func TestSimpleLinearMotion(t *testing.T) { inputSteps := []node{} ctx := context.Background() logger := logging.NewTestLogger(t) - m, err := referenceframe.ParseModelJSONFile(rutils.ResolveFile("components/arm/xarm/xarm7_kinematics.json"), "") + m, err := referenceframe.ParseModelJSONFile(rutils.ResolveFile("services/motion/data/xarm7_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) goalPos := spatialmath.NewPose(r3.Vector{X: 206, Y: 100, Z: 120.5}, &spatialmath.OrientationVectorDegrees{OY: -1}) diff --git a/motionplan/collision_test.go b/motionplan/collision_test.go index b32fbffbfb4..39772e82b40 100644 --- a/motionplan/collision_test.go +++ b/motionplan/collision_test.go @@ -73,7 +73,7 @@ func TestCheckCollisions(t *testing.T) { // case 2: zero position of xArm6 arm - should have number of collisions = to number of geometries - 1 // no external geometries considered, self collision only - m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") + m, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) gf, _ := m.Geometries(make([]frame.Input, len(m.DoF()))) test.That(t, gf, test.ShouldNotBeNil) @@ -83,7 +83,7 @@ func TestCheckCollisions(t *testing.T) { } func TestUniqueCollisions(t *testing.T) { - m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") + m, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) // zero position of xarm6 arm diff --git a/motionplan/constraint_test.go b/motionplan/constraint_test.go index aba13c99398..95ca7de1c13 100644 --- a/motionplan/constraint_test.go +++ b/motionplan/constraint_test.go @@ -52,7 +52,7 @@ func TestConstraintPath(t *testing.T) { homePos := frame.FloatsToInputs([]float64{0, 0, 0, 0, 0, 0}) toPos := frame.FloatsToInputs([]float64{0, 0, 0, 0, 0, 1}) - modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") + modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) ci := &ik.Segment{StartConfiguration: homePos, EndConfiguration: toPos, Frame: modelXarm} @@ -210,7 +210,7 @@ func TestCollisionConstraints(t *testing.T) { test.That(t, err, test.ShouldBeNil) // setup zero position as reference CollisionGraph and use it in handler - model, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") + model, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) fs := frame.NewEmptyFrameSystem("test") err = fs.AddFrame(model, fs.Frame(frame.World)) @@ -278,7 +278,7 @@ func BenchmarkCollisionConstraints(b *testing.B) { test.That(b, err, test.ShouldBeNil) // setup zero position as reference CollisionGraph and use it in handler - model, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") + model, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") test.That(b, err, test.ShouldBeNil) fs := frame.NewEmptyFrameSystem("test") err = fs.AddFrame(model, fs.Frame(frame.World)) diff --git a/motionplan/ik/nloptInverseKinematics_test.go b/motionplan/ik/nloptInverseKinematics_test.go index c3157f3b0ef..f449d4de37f 100644 --- a/motionplan/ik/nloptInverseKinematics_test.go +++ b/motionplan/ik/nloptInverseKinematics_test.go @@ -16,7 +16,7 @@ import ( func TestCreateNloptSolver(t *testing.T) { logger := logging.NewTestLogger(t) - m, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") + m, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) ik, err := CreateNloptSolver(m.DoF(), logger, -1, false, true) test.That(t, err, test.ShouldBeNil) diff --git a/motionplan/ik/solver_test.go b/motionplan/ik/solver_test.go index 536852604b9..fbcdd40b7f3 100644 --- a/motionplan/ik/solver_test.go +++ b/motionplan/ik/solver_test.go @@ -23,7 +23,7 @@ var ( func TestCombinedIKinematics(t *testing.T) { logger := logging.NewTestLogger(t) - m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") + m, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) ik, err := CreateCombinedIKFrameSolver(m, logger, nCPU, defaultGoalThreshold) test.That(t, err, test.ShouldBeNil) @@ -65,7 +65,7 @@ func TestUR5NloptIKinematics(t *testing.T) { func TestCombinedCPUs(t *testing.T) { logger := logging.NewTestLogger(t) - m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm7_kinematics.json"), "") + m, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm7_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) ik, err := CreateCombinedIKFrameSolver(m, logger, runtime.NumCPU()/400000, defaultGoalThreshold) test.That(t, err, test.ShouldBeNil) diff --git a/motionplan/kinematic_test.go b/motionplan/kinematic_test.go index a6e50e2448c..56193f5fc4a 100644 --- a/motionplan/kinematic_test.go +++ b/motionplan/kinematic_test.go @@ -17,7 +17,7 @@ import ( ) func BenchmarkFK(b *testing.B) { - m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm7_kinematics.json"), "") + m, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm7_kinematics_test.json"), "") test.That(b, err, test.ShouldBeNil) for n := 0; n < b.N; n++ { _, err := m.Transform(frame.FloatsToInputs(make([]float64, 7))) @@ -41,7 +41,7 @@ func TestForwardKinematics(t *testing.T) { test.That(t, spatial.PoseAlmostEqual(expect, pos), test.ShouldBeTrue) // Test the 6dof xarm we actually have - m, err = frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") + m, err = frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) // Confirm end effector starts at 207, 0, 112 @@ -159,7 +159,7 @@ func TestDeriv(t *testing.T) { func TestDynamicFrameSystemXArm(t *testing.T) { fs := frame.NewEmptyFrameSystem("test") - model, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") + model, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) fs.AddFrame(model, fs.World()) @@ -212,7 +212,7 @@ func TestComplicatedDynamicFrameSystem(t *testing.T) { fs.AddFrame(gantryY, gantryX) // xarm on gantry - modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") + modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) fs.AddFrame(modelXarm, gantryY) @@ -316,7 +316,7 @@ func TestKinematicsJSONvsURDF(t *testing.T) { } func TestComputeOOBPosition(t *testing.T) { - model, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "foo") + model, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "foo") test.That(t, err, test.ShouldBeNil) test.That(t, model.Name(), test.ShouldEqual, "foo") diff --git a/motionplan/motionPlanner_test.go b/motionplan/motionPlanner_test.go index 0b1c8fda434..abef120af8f 100644 --- a/motionplan/motionPlanner_test.go +++ b/motionplan/motionPlanner_test.go @@ -89,7 +89,7 @@ func TestConstrainedMotion(t *testing.T) { // TestConstrainedArmMotion tests a simple linear motion on a longer path, with a no-spill constraint. func constrainedXArmMotion() (*planConfig, error) { - model, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm7_kinematics.json"), "") + model, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm7_kinematics_test.json"), "") if err != nil { return nil, err } @@ -270,7 +270,7 @@ func simple2DMap() (*planConfig, error) { // simpleArmMotion tests moving an xArm7. func simpleXArmMotion() (*planConfig, error) { - xarm, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm7_kinematics.json"), "") + xarm, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm7_kinematics_test.json"), "") if err != nil { return nil, err } @@ -450,7 +450,7 @@ func makeTestFS(t *testing.T) frame.FrameSystem { test.That(t, err, test.ShouldBeNil) fs.AddFrame(gantryY, gantryX) - modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") + modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) fs.AddFrame(modelXarm, gantryY) @@ -584,7 +584,7 @@ func TestMultiArmSolve(t *testing.T) { func TestReachOverArm(t *testing.T) { // setup frame system with an xarm - xarm, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") + xarm, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) offset, err := frame.NewStaticFrame("offset", spatialmath.NewPoseFromPoint(r3.Vector{X: -500, Y: 200})) test.That(t, err, test.ShouldBeNil) @@ -740,7 +740,7 @@ func TestSolverFrameGeometries(t *testing.T) { func TestArmConstraintSpecificationSolve(t *testing.T) { fs := frame.NewEmptyFrameSystem("") - x, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") + x, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) test.That(t, fs.AddFrame(x, fs.World()), test.ShouldBeNil) bc, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{Z: 100}), r3.Vector{200, 200, 200}, "") diff --git a/referenceframe/model_json_test.go b/referenceframe/model_json_test.go index 2430c261a98..6e1684c80e7 100644 --- a/referenceframe/model_json_test.go +++ b/referenceframe/model_json_test.go @@ -15,8 +15,8 @@ import ( func TestParseJSONFile(t *testing.T) { goodFiles := []string{ "components/arm/eva/eva_kinematics.json", - "components/arm/xarm/xarm6_kinematics.json", - "components/arm/xarm/xarm7_kinematics.json", + "services/motion/data/xarm6_kinematics_test.json", + "services/motion/data/xarm7_kinematics_test.json", "referenceframe/testjson/ur5eDH.json", "components/arm/universalrobots/ur5e.json", "components/arm/fake/dofbot.json", diff --git a/referenceframe/model_test.go b/referenceframe/model_test.go index 6bda57de026..5c049e36aa7 100644 --- a/referenceframe/model_test.go +++ b/referenceframe/model_test.go @@ -13,7 +13,7 @@ import ( ) func TestModelLoading(t *testing.T) { - m, err := ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") + m, err := ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) test.That(t, m.Name(), test.ShouldEqual, "xArm6") simpleM, ok := m.(*SimpleModel) @@ -33,13 +33,13 @@ func TestModelLoading(t *testing.T) { randpos := GenerateRandomConfiguration(m, rand.New(rand.NewSource(1))) test.That(t, simpleM.validInputs(FloatsToInputs(randpos)), test.ShouldBeNil) - m, err = ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "foo") + m, err = ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "foo") test.That(t, err, test.ShouldBeNil) test.That(t, m.Name(), test.ShouldEqual, "foo") } func TestTransform(t *testing.T) { - m, err := ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") + m, err := ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) simpleM, ok := m.(*SimpleModel) test.That(t, ok, test.ShouldBeTrue) @@ -68,7 +68,7 @@ func TestTransform(t *testing.T) { } func TestIncorrectInputs(t *testing.T) { - m, err := ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") + m, err := ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) dof := len(m.DoF()) diff --git a/services/motion/data/xarm6_kinematics_test.json b/services/motion/data/xarm6_kinematics_test.json new file mode 100644 index 00000000000..cb4c8af962d --- /dev/null +++ b/services/motion/data/xarm6_kinematics_test.json @@ -0,0 +1,217 @@ +{ + "name": "xArm6", + "links": [ + { + "id": "base", + "parent": "world", + "translation": { + "x": 0, + "y": 0, + "z": 0 + } + }, + { + "id": "base_top", + "parent": "waist", + "translation": { + "x": 0, + "y": 0, + "z": 267 + }, + "geometry": { + "r": 50, + "l": 320, + "translation": { + "x": 0, + "y": 0, + "z": 160 + } + } + }, + { + "id": "upper_arm", + "parent": "shoulder", + "translation": { + "x": 53.5, + "y": 0, + "z": 284.5 + }, + "geometry": { + "x": 110, + "y": 190, + "z": 370, + "translation": { + "x": 0, + "y": 0, + "z": 135 + } + } + }, + { + "id": "upper_forearm", + "parent": "elbow", + "translation": { + "x": 77.5, + "y": 0, + "z": -172.5 + }, + "geometry": { + "x": 100, + "y": 190, + "z": 250, + "translation": { + "x": 49.49, + "y": 0, + "z": -49.49 + }, + "orientation": { + "type": "ov_degrees", + "value": { + "x": 0.707106, + "y": 0, + "z": -0.707106, + "th": 0 + } + } + } + }, + { + "id": "lower_forearm", + "parent": "forearm_rot", + "translation": { + "x": 0, + "y": 0, + "z": -170 + }, + "geometry": { + "r": 45, + "l": 285, + "translation": { + "x": 0, + "y": -27.5, + "z": -104.8 + }, + "orientation": { + "type": "ov_degrees", + "value": { + "th": -90, + "x": 0, + "y": 0.2537568, + "z": 0.9672615 + } + } + } + }, + { + "id": "wrist_link", + "parent": "wrist", + "translation": { + "x": 76, + "y": 0, + "z": -97 + }, + "geometry": { + "x": 150, + "y": 100, + "z": 135, + "translation": { + "x": 75, + "y": 10, + "z": -67.5 + } + } + }, + { + "id": "gripper_mount", + "parent": "gripper_rot", + "translation": { + "x": 0, + "y": 0, + "z": 0 + }, + "orientation": { + "type": "ov_degrees", + "value": { + "x": 0, + "y": 0, + "z": -1, + "th": 0 + } + } + } + ], + "joints": [ + { + "id": "waist", + "type": "revolute", + "parent": "base", + "axis": { + "x": 0, + "y": 0, + "z": 1 + }, + "max": 359, + "min": -359 + }, + { + "id": "shoulder", + "type": "revolute", + "parent": "base_top", + "axis": { + "x": 0, + "y": 1, + "z": 0 + }, + "max": 120, + "min": -118 + }, + { + "id": "elbow", + "type": "revolute", + "parent": "upper_arm", + "axis": { + "x": 0, + "y": 1, + "z": 0 + }, + "max": 10, + "min": -225 + }, + { + "id": "forearm_rot", + "type": "revolute", + "parent": "upper_forearm", + "axis": { + "x": 0, + "y": 0, + "z": -1 + }, + "max": 359, + "min": -359 + }, + { + "id": "wrist", + "type": "revolute", + "parent": "lower_forearm", + "axis": { + "x": 0, + "y": 1, + "z": 0 + }, + "max": 179, + "min": -97 + }, + { + "id": "gripper_rot", + "type": "revolute", + "parent": "wrist_link", + "axis": { + "x": 0, + "y": 0, + "z": -1 + }, + "max": 359, + "min": -359 + } + ] +} diff --git a/services/motion/data/xarm7_kinematics_test.json b/services/motion/data/xarm7_kinematics_test.json new file mode 100644 index 00000000000..344f79e5eed --- /dev/null +++ b/services/motion/data/xarm7_kinematics_test.json @@ -0,0 +1,257 @@ +{ + "name": "xArm7", + "links": [ + { + "id": "base", + "parent": "world", + "translation": { + "x": 0, + "y": 0, + "z": 0 + } + }, + { + "id": "base_top", + "parent": "waist", + "translation": { + "x": 0, + "y": 0, + "z": 267 + }, + "geometry": { + "r": 50, + "l": 320, + "translation": { + "x": 0, + "y": 0, + "z": 160 + } + } + }, + { + "id": "base_arm_link", + "parent": "shoulder", + "translation": { + "x": 0, + "y": 0, + "z": 200 + }, + "geometry": { + "x": 110, + "y": 190, + "z": 220, + "translation": { + "x": 0, + "y": 30, + "z": 100 + } + } + }, + { + "id": "upper_arm", + "parent": "upper_arm_rot", + "translation": { + "x": 52.5, + "y": 0, + "z": 93 + }, + "geometry": { + "x": 100, + "y": 130, + "z": 180, + "translation": { + "x": 32.88, + "y": -10, + "z": 32.88 + }, + "orientation": { + "type": "ov_degrees", + "value": { + "x": 0.707106, + "y": 0, + "z": 0.707106, + "th": 0 + } + } + } + }, + { + "id": "upper_forearm", + "parent": "elbow", + "translation": { + "x": 77.5, + "y": 0, + "z": -172.5 + }, + "geometry": { + "x": 100, + "y": 150, + "z": 250, + "translation": { + "x": 60.98, + "y": -37.5, + "z": -60.98 + }, + "orientation": { + "type": "ov_degrees", + "value": { + "x": 0.707106, + "y": 0, + "z": -0.707106, + "th": 0 + } + } + } + }, + { + "id": "lower_forearm", + "parent": "forearm_rot", + "translation": { + "x": 0, + "y": 0, + "z": -170 + }, + "geometry": { + "r": 45, + "l": 285, + "translation": { + "x": 0, + "y": -27.5, + "z": -104.8 + }, + "orientation": { + "type": "ov_degrees", + "value": { + "th": -90, + "x": 0, + "y": 0.2537568, + "z": 0.9672615 + } + } + } + }, + { + "id": "wrist_link", + "parent": "wrist", + "translation": { + "x": 76, + "y": 0, + "z": -97 + }, + "geometry": { + "x": 150, + "y": 100, + "z": 135, + "translation": { + "x": 75, + "y": 0, + "z": -67.5 + } + } + }, + { + "id": "gripper_mount", + "parent": "gripper_rot", + "translation": { + "x": 0, + "y": 0, + "z": 0 + }, + "orientation": { + "type": "ov_degrees", + "value": { + "x": 0, + "y": 0, + "z": -1, + "th": 0 + } + } + } + ], + "joints": [ + { + "id": "waist", + "type": "revolute", + "parent": "base", + "axis": { + "x": 0, + "y": 0, + "z": 1 + }, + "max": 359, + "min": -359 + }, + { + "id": "shoulder", + "type": "revolute", + "parent": "base_top", + "axis": { + "x": 0, + "y": 1, + "z": 0 + }, + "max": 120, + "min": -118 + }, + { + "id": "upper_arm_rot", + "type": "revolute", + "parent": "base_arm_link", + "axis": { + "x": 0, + "y": 0, + "z": 1 + }, + "max": 359, + "min": -359 + }, + { + "id": "elbow", + "type": "revolute", + "parent": "upper_arm", + "axis": { + "x": 0, + "y": -1, + "z": 0 + }, + "max": 225, + "min": -11 + }, + { + "id": "forearm_rot", + "type": "revolute", + "parent": "upper_forearm", + "axis": { + "x": 0, + "y": 0, + "z": -1 + }, + "max": 359, + "min": -359 + }, + { + "id": "wrist", + "type": "revolute", + "parent": "lower_forearm", + "axis": { + "x": 0, + "y": 1, + "z": 0 + }, + "max": 179, + "min": -97 + }, + { + "id": "gripper_rot", + "type": "revolute", + "parent": "wrist_link", + "axis": { + "x": 0, + "y": 0, + "z": -1 + }, + "max": 359, + "min": -359 + } + ] +} From 390948322f28cec252302047c50d7ac6407bed92 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 21 Nov 2024 17:41:36 -0500 Subject: [PATCH 05/13] missed updating the import part in two places -- updated now --- motionplan/constraint_test.go | 2 +- motionplan/motionPlanner_test.go | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/motionplan/constraint_test.go b/motionplan/constraint_test.go index 95ca7de1c13..9f551580820 100644 --- a/motionplan/constraint_test.go +++ b/motionplan/constraint_test.go @@ -139,7 +139,7 @@ func TestLineFollow(t *testing.T) { fs := frame.NewEmptyFrameSystem("test") - m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm7_kinematics.json"), "") + m, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm7_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) err = fs.AddFrame(m, fs.World()) diff --git a/motionplan/motionPlanner_test.go b/motionplan/motionPlanner_test.go index abef120af8f..a7e45832de2 100644 --- a/motionplan/motionPlanner_test.go +++ b/motionplan/motionPlanner_test.go @@ -1225,7 +1225,7 @@ func TestArmGantryCheckPlan(t *testing.T) { err = fs.AddFrame(gantryX, gantryOffset) test.That(t, err, test.ShouldBeNil) - modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") + modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) err = fs.AddFrame(modelXarm, gantryX) test.That(t, err, test.ShouldBeNil) From e6be575668cfd86f03727a4e750f3e9ad4b93704 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 22 Nov 2024 10:56:34 -0500 Subject: [PATCH 06/13] remove module dependency + move kinematics file into fake folder so we can embed them to replicate module functionality --- components/arm/fake/fake.go | 36 ++- .../arm/fake/lite6_kinematics_test.json | 217 ++++++++++++++++++ .../arm/fake}/xarm6_kinematics_test.json | 0 .../arm/fake}/xarm7_kinematics_test.json | 0 components/arm/wrapper/wrapper_test.go | 4 +- motionplan/cBiRRT_test.go | 2 +- motionplan/collision_test.go | 5 +- motionplan/constraint_test.go | 8 +- motionplan/ik/nloptInverseKinematics_test.go | 2 +- motionplan/ik/solver_test.go | 4 +- motionplan/kinematic_test.go | 10 +- motionplan/motionPlanner_test.go | 12 +- referenceframe/model_json_test.go | 4 +- referenceframe/model_test.go | 8 +- 14 files changed, 280 insertions(+), 32 deletions(-) create mode 100644 components/arm/fake/lite6_kinematics_test.json rename {services/motion/data => components/arm/fake}/xarm6_kinematics_test.json (100%) rename {services/motion/data => components/arm/fake}/xarm7_kinematics_test.json (100%) diff --git a/components/arm/fake/fake.go b/components/arm/fake/fake.go index b862be410a1..69e57b37a64 100644 --- a/components/arm/fake/fake.go +++ b/components/arm/fake/fake.go @@ -4,11 +4,12 @@ package fake import ( "context" _ "embed" + "fmt" "strings" "sync" "github.com/pkg/errors" - xarm "github.com/viam-modules/viam-ufactory-xarm/arm" + // xarm "github.com/viam-modules/viam-ufactory-xarm/arm" "go.viam.com/rdk/components/arm" "go.viam.com/rdk/components/arm/eva" @@ -35,6 +36,21 @@ var fakejson []byte //go:embed dofbot.json var dofbotjson []byte +//go:embed xarm6_kinematics_test.json +var xArm6modeljson []byte + +//go:embed xarm7_kinematics_test.json +var xArm7modeljson []byte + +//go:embed lite6_kinematics_test.json +var lite6modeljson []byte + +const ( + xArmModelName6DOF = "xArm6" + xArmModelName7DOF = "xArm7" + xArmModelNameLite = "lite6" +) + // Config is used for converting config attributes. type Config struct { ArmModel string `json:"arm-model,omitempty"` @@ -256,8 +272,8 @@ func (a *Arm) Geometries(ctx context.Context, extra map[string]interface{}) ([]s func modelFromName(model, name string) (referenceframe.Model, error) { switch model { - case xarm.ModelName6DOF, xarm.ModelName7DOF, xarm.ModelNameLite: - return xarm.MakeModelFrame(name, model) + case xArmModelName6DOF, xArmModelName7DOF, xArmModelNameLite: + return xArmMakeModelFrame(name, model) case ur.Model.Name: return ur.MakeModelFrame(name) case eva.Model.Name: @@ -281,3 +297,17 @@ func modelFromPath(modelPath, name string) (referenceframe.Model, error) { return nil, errors.New("only files with .json and .urdf file extensions are supported") } } + +// xArmMakeModelFrame returns the kinematics model of the xarm arm, which has all Frame information. +func xArmMakeModelFrame(name, modelName string) (referenceframe.Model, error) { + switch modelName { + case xArmModelName6DOF: + return referenceframe.UnmarshalModelJSON(xArm6modeljson, name) + case xArmModelNameLite: + return referenceframe.UnmarshalModelJSON(lite6modeljson, name) + case xArmModelName7DOF: + return referenceframe.UnmarshalModelJSON(xArm7modeljson, name) + default: + return nil, fmt.Errorf("no kinematics information for xarm of model %s", modelName) + } +} diff --git a/components/arm/fake/lite6_kinematics_test.json b/components/arm/fake/lite6_kinematics_test.json new file mode 100644 index 00000000000..0498017440b --- /dev/null +++ b/components/arm/fake/lite6_kinematics_test.json @@ -0,0 +1,217 @@ +{ + "name": "lite6", + "links": [ + { + "id": "base", + "parent": "world", + "translation": { + "x": 0, + "y": 0, + "z": 0 + } + }, + { + "id": "base_top", + "parent": "waist", + "translation": { + "x": 0, + "y": 0, + "z": 243.3 + }, + "geometry": { + "r": 40, + "l": 290, + "translation": { + "x": 0, + "y": 0, + "z": 145 + } + } + }, + { + "id": "upper_arm", + "parent": "shoulder", + "translation": { + "x": 0, + "y": 0, + "z": 200 + }, + "geometry": { + "r": 40, + "l": 280, + "translation": { + "x": 0, + "y": 105, + "z": 140 + } + } + }, + { + "id": "upper_forearm", + "parent": "elbow", + "translation": { + "x": 87, + "y": 0, + "z": 0 + }, + "geometry": { + "x": 165, + "y": 85, + "z": 85, + "translation": { + "x": 43.5, + "y": 0, + "z": 0 + } + } + }, + { + "id": "lower_forearm_filler", + "parent": "forearm_rot", + "translation": { + "x": 0, + "y": 0, + "z": 0 + }, + "geometry": { + "r": 40, + "l": 200, + "translation": { + "x": 0, + "y": 0, + "z": -30 + } + } + }, + { + "id": "lower_forearm", + "parent": "lower_forearm_filler", + "translation": { + "x": 0, + "y": 0, + "z": -227.6 + }, + "geometry": { + "x": 75, + "y": 35, + "z": 180, + "translation": { + "x": 0, + "y": 60, + "z": -175 + } + } + }, + { + "id": "wrist_link", + "parent": "wrist", + "translation": { + "x": 0, + "y": 0, + "z": -61.5 + }, + "geometry": { + "x": 75, + "y": 75, + "z": 100, + "translation": { + "x": 0, + "y": 0, + "z": -50 + } + } + }, + { + "id": "gripper_mount", + "parent": "gripper_rot", + "translation": { + "x": 0, + "y": 0, + "z": 0 + }, + "orientation": { + "type": "ov_degrees", + "value": { + "x": 0, + "y": 0, + "z": -1, + "th": 0 + } + } + } + ], + "joints": [ + { + "id": "waist", + "type": "revolute", + "parent": "base", + "axis": { + "x": 0, + "y": 0, + "z": 1 + }, + "max": 359, + "min": -359 + }, + { + "id": "shoulder", + "type": "revolute", + "parent": "base_top", + "axis": { + "x": 0, + "y": 1, + "z": 0 + }, + "max": 149, + "min": -149 + }, + { + "id": "elbow", + "type": "revolute", + "parent": "upper_arm", + "axis": { + "x": 0, + "y": -1, + "z": 0 + }, + "max": 299, + "min": -3 + }, + { + "id": "forearm_rot", + "type": "revolute", + "parent": "upper_forearm", + "axis": { + "x": 0, + "y": 0, + "z": -1 + }, + "max": 359, + "min": -359 + }, + { + "id": "wrist", + "type": "revolute", + "parent": "lower_forearm", + "axis": { + "x": 0, + "y": 1, + "z": 0 + }, + "max": 124, + "min": -124 + }, + { + "id": "gripper_rot", + "type": "revolute", + "parent": "wrist_link", + "axis": { + "x": 0, + "y": 0, + "z": -1 + }, + "max": 359, + "min": -359 + } + ] +} diff --git a/services/motion/data/xarm6_kinematics_test.json b/components/arm/fake/xarm6_kinematics_test.json similarity index 100% rename from services/motion/data/xarm6_kinematics_test.json rename to components/arm/fake/xarm6_kinematics_test.json diff --git a/services/motion/data/xarm7_kinematics_test.json b/components/arm/fake/xarm7_kinematics_test.json similarity index 100% rename from services/motion/data/xarm7_kinematics_test.json rename to components/arm/fake/xarm7_kinematics_test.json diff --git a/components/arm/wrapper/wrapper_test.go b/components/arm/wrapper/wrapper_test.go index 859f00aaae1..5b07a8c4b3d 100644 --- a/components/arm/wrapper/wrapper_test.go +++ b/components/arm/wrapper/wrapper_test.go @@ -27,7 +27,7 @@ func TestReconfigure(t *testing.T) { cfg1 := resource.Config{ Name: "testArm", ConvertedAttributes: &Config{ - ModelFilePath: "../../../services/motion/data/xarm6_kinematics_test.json", + ModelFilePath: "../fake/xarm6_kinematics_test.json", ArmName: armName.ShortName(), }, } @@ -35,7 +35,7 @@ func TestReconfigure(t *testing.T) { cfg1Err := resource.Config{ Name: "testArm", ConvertedAttributes: &Config{ - ModelFilePath: "../../../services/motion/data/xarm6_kinematics_test.json", + ModelFilePath: "../fake/xarm6_kinematics_test.json", ArmName: "dne1", }, } diff --git a/motionplan/cBiRRT_test.go b/motionplan/cBiRRT_test.go index 66fa579f6c9..342c52625c9 100644 --- a/motionplan/cBiRRT_test.go +++ b/motionplan/cBiRRT_test.go @@ -33,7 +33,7 @@ func TestSimpleLinearMotion(t *testing.T) { inputSteps := []node{} ctx := context.Background() logger := logging.NewTestLogger(t) - m, err := referenceframe.ParseModelJSONFile(rutils.ResolveFile("services/motion/data/xarm7_kinematics_test.json"), "") + m, err := referenceframe.ParseModelJSONFile(rutils.ResolveFile("components/arm/fake/xarm7_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) goalPos := spatialmath.NewPose(r3.Vector{X: 206, Y: 100, Z: 120.5}, &spatialmath.OrientationVectorDegrees{OY: -1}) diff --git a/motionplan/collision_test.go b/motionplan/collision_test.go index 39772e82b40..13fdd51530f 100644 --- a/motionplan/collision_test.go +++ b/motionplan/collision_test.go @@ -73,7 +73,8 @@ func TestCheckCollisions(t *testing.T) { // case 2: zero position of xArm6 arm - should have number of collisions = to number of geometries - 1 // no external geometries considered, self collision only - m, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") + + m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) gf, _ := m.Geometries(make([]frame.Input, len(m.DoF()))) test.That(t, gf, test.ShouldNotBeNil) @@ -83,7 +84,7 @@ func TestCheckCollisions(t *testing.T) { } func TestUniqueCollisions(t *testing.T) { - m, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") + m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) // zero position of xarm6 arm diff --git a/motionplan/constraint_test.go b/motionplan/constraint_test.go index 9f551580820..1b8dc65ba24 100644 --- a/motionplan/constraint_test.go +++ b/motionplan/constraint_test.go @@ -52,7 +52,7 @@ func TestConstraintPath(t *testing.T) { homePos := frame.FloatsToInputs([]float64{0, 0, 0, 0, 0, 0}) toPos := frame.FloatsToInputs([]float64{0, 0, 0, 0, 0, 1}) - modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") + modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) ci := &ik.Segment{StartConfiguration: homePos, EndConfiguration: toPos, Frame: modelXarm} @@ -139,7 +139,7 @@ func TestLineFollow(t *testing.T) { fs := frame.NewEmptyFrameSystem("test") - m, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm7_kinematics_test.json"), "") + m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm7_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) err = fs.AddFrame(m, fs.World()) @@ -210,7 +210,7 @@ func TestCollisionConstraints(t *testing.T) { test.That(t, err, test.ShouldBeNil) // setup zero position as reference CollisionGraph and use it in handler - model, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") + model, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) fs := frame.NewEmptyFrameSystem("test") err = fs.AddFrame(model, fs.Frame(frame.World)) @@ -278,7 +278,7 @@ func BenchmarkCollisionConstraints(b *testing.B) { test.That(b, err, test.ShouldBeNil) // setup zero position as reference CollisionGraph and use it in handler - model, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") + model, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm6_kinematics_test.json"), "") test.That(b, err, test.ShouldBeNil) fs := frame.NewEmptyFrameSystem("test") err = fs.AddFrame(model, fs.Frame(frame.World)) diff --git a/motionplan/ik/nloptInverseKinematics_test.go b/motionplan/ik/nloptInverseKinematics_test.go index f449d4de37f..33fffe08140 100644 --- a/motionplan/ik/nloptInverseKinematics_test.go +++ b/motionplan/ik/nloptInverseKinematics_test.go @@ -16,7 +16,7 @@ import ( func TestCreateNloptSolver(t *testing.T) { logger := logging.NewTestLogger(t) - m, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") + m, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) ik, err := CreateNloptSolver(m.DoF(), logger, -1, false, true) test.That(t, err, test.ShouldBeNil) diff --git a/motionplan/ik/solver_test.go b/motionplan/ik/solver_test.go index fbcdd40b7f3..c1327623905 100644 --- a/motionplan/ik/solver_test.go +++ b/motionplan/ik/solver_test.go @@ -23,7 +23,7 @@ var ( func TestCombinedIKinematics(t *testing.T) { logger := logging.NewTestLogger(t) - m, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") + m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) ik, err := CreateCombinedIKFrameSolver(m, logger, nCPU, defaultGoalThreshold) test.That(t, err, test.ShouldBeNil) @@ -65,7 +65,7 @@ func TestUR5NloptIKinematics(t *testing.T) { func TestCombinedCPUs(t *testing.T) { logger := logging.NewTestLogger(t) - m, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm7_kinematics_test.json"), "") + m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm7_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) ik, err := CreateCombinedIKFrameSolver(m, logger, runtime.NumCPU()/400000, defaultGoalThreshold) test.That(t, err, test.ShouldBeNil) diff --git a/motionplan/kinematic_test.go b/motionplan/kinematic_test.go index 56193f5fc4a..e5ed5f24b5e 100644 --- a/motionplan/kinematic_test.go +++ b/motionplan/kinematic_test.go @@ -17,7 +17,7 @@ import ( ) func BenchmarkFK(b *testing.B) { - m, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm7_kinematics_test.json"), "") + m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm7_kinematics_test.json"), "") test.That(b, err, test.ShouldBeNil) for n := 0; n < b.N; n++ { _, err := m.Transform(frame.FloatsToInputs(make([]float64, 7))) @@ -41,7 +41,7 @@ func TestForwardKinematics(t *testing.T) { test.That(t, spatial.PoseAlmostEqual(expect, pos), test.ShouldBeTrue) // Test the 6dof xarm we actually have - m, err = frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") + m, err = frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fakexarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) // Confirm end effector starts at 207, 0, 112 @@ -159,7 +159,7 @@ func TestDeriv(t *testing.T) { func TestDynamicFrameSystemXArm(t *testing.T) { fs := frame.NewEmptyFrameSystem("test") - model, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") + model, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fakexarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) fs.AddFrame(model, fs.World()) @@ -212,7 +212,7 @@ func TestComplicatedDynamicFrameSystem(t *testing.T) { fs.AddFrame(gantryY, gantryX) // xarm on gantry - modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") + modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fakexarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) fs.AddFrame(modelXarm, gantryY) @@ -316,7 +316,7 @@ func TestKinematicsJSONvsURDF(t *testing.T) { } func TestComputeOOBPosition(t *testing.T) { - model, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "foo") + model, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fakexarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) test.That(t, model.Name(), test.ShouldEqual, "foo") diff --git a/motionplan/motionPlanner_test.go b/motionplan/motionPlanner_test.go index a7e45832de2..956888cec1c 100644 --- a/motionplan/motionPlanner_test.go +++ b/motionplan/motionPlanner_test.go @@ -89,7 +89,7 @@ func TestConstrainedMotion(t *testing.T) { // TestConstrainedArmMotion tests a simple linear motion on a longer path, with a no-spill constraint. func constrainedXArmMotion() (*planConfig, error) { - model, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm7_kinematics_test.json"), "") + model, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm7_kinematics_test.json"), "") if err != nil { return nil, err } @@ -270,7 +270,7 @@ func simple2DMap() (*planConfig, error) { // simpleArmMotion tests moving an xArm7. func simpleXArmMotion() (*planConfig, error) { - xarm, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm7_kinematics_test.json"), "") + xarm, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm7_kinematics_test.json"), "") if err != nil { return nil, err } @@ -450,7 +450,7 @@ func makeTestFS(t *testing.T) frame.FrameSystem { test.That(t, err, test.ShouldBeNil) fs.AddFrame(gantryY, gantryX) - modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") + modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fakexarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) fs.AddFrame(modelXarm, gantryY) @@ -584,7 +584,7 @@ func TestMultiArmSolve(t *testing.T) { func TestReachOverArm(t *testing.T) { // setup frame system with an xarm - xarm, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") + xarm, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fakexarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) offset, err := frame.NewStaticFrame("offset", spatialmath.NewPoseFromPoint(r3.Vector{X: -500, Y: 200})) test.That(t, err, test.ShouldBeNil) @@ -740,7 +740,7 @@ func TestSolverFrameGeometries(t *testing.T) { func TestArmConstraintSpecificationSolve(t *testing.T) { fs := frame.NewEmptyFrameSystem("") - x, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") + x, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) test.That(t, fs.AddFrame(x, fs.World()), test.ShouldBeNil) bc, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{Z: 100}), r3.Vector{200, 200, 200}, "") @@ -1225,7 +1225,7 @@ func TestArmGantryCheckPlan(t *testing.T) { err = fs.AddFrame(gantryX, gantryOffset) test.That(t, err, test.ShouldBeNil) - modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") + modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) err = fs.AddFrame(modelXarm, gantryX) test.That(t, err, test.ShouldBeNil) diff --git a/referenceframe/model_json_test.go b/referenceframe/model_json_test.go index 6e1684c80e7..21aeaaefbbe 100644 --- a/referenceframe/model_json_test.go +++ b/referenceframe/model_json_test.go @@ -15,8 +15,8 @@ import ( func TestParseJSONFile(t *testing.T) { goodFiles := []string{ "components/arm/eva/eva_kinematics.json", - "services/motion/data/xarm6_kinematics_test.json", - "services/motion/data/xarm7_kinematics_test.json", + "components/arm/fake/xarm6_kinematics_test.json", + "components/arm/fake/xarm7_kinematics_test.json", "referenceframe/testjson/ur5eDH.json", "components/arm/universalrobots/ur5e.json", "components/arm/fake/dofbot.json", diff --git a/referenceframe/model_test.go b/referenceframe/model_test.go index 5c049e36aa7..9b6229f576f 100644 --- a/referenceframe/model_test.go +++ b/referenceframe/model_test.go @@ -13,7 +13,7 @@ import ( ) func TestModelLoading(t *testing.T) { - m, err := ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") + m, err := ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) test.That(t, m.Name(), test.ShouldEqual, "xArm6") simpleM, ok := m.(*SimpleModel) @@ -33,13 +33,13 @@ func TestModelLoading(t *testing.T) { randpos := GenerateRandomConfiguration(m, rand.New(rand.NewSource(1))) test.That(t, simpleM.validInputs(FloatsToInputs(randpos)), test.ShouldBeNil) - m, err = ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "foo") + m, err = ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm6_kinematics_test.json"), "foo") test.That(t, err, test.ShouldBeNil) test.That(t, m.Name(), test.ShouldEqual, "foo") } func TestTransform(t *testing.T) { - m, err := ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") + m, err := ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) simpleM, ok := m.(*SimpleModel) test.That(t, ok, test.ShouldBeTrue) @@ -68,7 +68,7 @@ func TestTransform(t *testing.T) { } func TestIncorrectInputs(t *testing.T) { - m, err := ParseModelJSONFile(utils.ResolveFile("services/motion/data/xarm6_kinematics_test.json"), "") + m, err := ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) dof := len(m.DoF()) From 0ec7b529c6005000db0c89d4cf714f74e74e6e40 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 22 Nov 2024 10:58:17 -0500 Subject: [PATCH 07/13] change min value from 118 to 117.5 for the xarm7 kinematics file --- components/arm/fake/xarm7_kinematics_test.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/components/arm/fake/xarm7_kinematics_test.json b/components/arm/fake/xarm7_kinematics_test.json index 344f79e5eed..a60da81ddd1 100644 --- a/components/arm/fake/xarm7_kinematics_test.json +++ b/components/arm/fake/xarm7_kinematics_test.json @@ -191,7 +191,7 @@ "z": 0 }, "max": 120, - "min": -118 + "min": -117.5 }, { "id": "upper_arm_rot", From f0a2e33b690435f270516502c86ac3ca0e760457 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 22 Nov 2024 10:58:50 -0500 Subject: [PATCH 08/13] update go.mod and go.sum --- go.mod | 3 +-- go.sum | 3 --- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/go.mod b/go.mod index 9346538622b..54171d0a13a 100644 --- a/go.mod +++ b/go.mod @@ -160,6 +160,7 @@ require ( github.com/bufbuild/protoyaml-go v0.1.8 // indirect github.com/butuzov/ireturn v0.3.0 // indirect github.com/butuzov/mirror v1.2.0 // indirect + github.com/bytedance/sonic v1.12.2 // indirect github.com/bytedance/sonic/loader v0.2.1 // indirect github.com/campoy/embedmd v1.0.0 // indirect github.com/catenacyber/perfsprint v0.7.1 // indirect @@ -180,7 +181,6 @@ require ( github.com/chewxy/hm v1.0.0 // indirect github.com/chewxy/math32 v1.0.8 // indirect github.com/ckaznocha/intrange v0.2.0 // indirect - github.com/cloudwego/base64x v0.1.4 // indirect github.com/cncf/xds/go v0.0.0-20240423153145-555b57ec207b // indirect github.com/containerd/stargz-snapshotter/estargz v0.15.1 // indirect github.com/cpuguy83/go-md2man/v2 v2.0.4 // indirect @@ -440,7 +440,6 @@ require ( github.com/erikstmartin/go-testdb v0.0.0-20160219214506-8d10e4a1bae5 // indirect github.com/kylelemons/go-gypsy v1.0.0 // indirect github.com/pkg/errors v0.9.1 - github.com/viam-modules/viam-ufactory-xarm v0.0.0-20241121162723-0285e0118688 github.com/ziutek/mymysql v1.5.4 // indirect golang.org/x/exp v0.0.0-20240904232852-e7e105dedf7e ) diff --git a/go.sum b/go.sum index e4bc9a77431..56a8f80eeb6 100644 --- a/go.sum +++ b/go.sum @@ -218,7 +218,6 @@ github.com/butuzov/mirror v1.2.0 h1:9YVK1qIjNspaqWutSv8gsge2e/Xpq1eqEkslEUHy5cs= github.com/butuzov/mirror v1.2.0/go.mod h1:DqZZDtzm42wIAIyHXeN8W/qb1EPlb9Qn/if9icBOpdQ= github.com/bytedance/sonic v1.12.2 h1:oaMFuRTpMHYLpCntGca65YWt5ny+wAceDERTkT2L9lg= github.com/bytedance/sonic v1.12.2/go.mod h1:B8Gt/XvtZ3Fqj+iSKMypzymZxw/FVwgIGKzMzT9r/rk= -github.com/bytedance/sonic/loader v0.1.1/go.mod h1:ncP89zfokxS5LZrJxl5z0UJcsk4M4yY2JpfqGeCtNLU= github.com/bytedance/sonic/loader v0.2.1 h1:1GgorWTqf12TA8mma4DDSbaQigE2wOgQo7iCjjJv3+E= github.com/bytedance/sonic/loader v0.2.1/go.mod h1:ncP89zfokxS5LZrJxl5z0UJcsk4M4yY2JpfqGeCtNLU= github.com/campoy/embedmd v1.0.0 h1:V4kI2qTJJLf4J29RzI/MAt2c3Bl4dQSYPuflzwFH2hY= @@ -1427,8 +1426,6 @@ github.com/valyala/quicktemplate v1.6.3/go.mod h1:fwPzK2fHuYEODzJ9pkw0ipCPNHZ2tD github.com/valyala/tcplisten v0.0.0-20161114210144-ceec8f93295a/go.mod h1:v3UYOV9WzVtRmSR+PDvWpU/qWl4Wa5LApYYX4ZtKbio= github.com/vbatts/tar-split v0.11.5 h1:3bHCTIheBm1qFTcgh9oPu+nNBtX+XJIupG/vacinCts= github.com/vbatts/tar-split v0.11.5/go.mod h1:yZbwRsSeGjusneWgA781EKej9HF8vme8okylkAeNKLk= -github.com/viam-modules/viam-ufactory-xarm v0.0.0-20241121162723-0285e0118688 h1:bzAd7S6+Ln0qyD2H69oVYLhA6oCNHYiyvX2+m12SU6w= -github.com/viam-modules/viam-ufactory-xarm v0.0.0-20241121162723-0285e0118688/go.mod h1:nd/mMK59zmB2tNAGreT9ge68MQYB+FU0xYmo+YBvpGg= github.com/viamrobotics/evdev v0.1.3 h1:mR4HFafvbc5Wx4Vp1AUJp6/aITfVx9AKyXWx+rWjpfc= github.com/viamrobotics/evdev v0.1.3/go.mod h1:N6nuZmPz7HEIpM7esNWwLxbYzqWqLSZkfI/1Sccckqk= github.com/viamrobotics/webrtc/v3 v3.99.10 h1:ykE14wm+HkqMD5Ozq4rvhzzfvnXAu14ak/HzA1OCzfY= From ddf903228dbe5163a5f3240a1206f35524b8dfd8 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 22 Nov 2024 10:59:53 -0500 Subject: [PATCH 09/13] update go.mod and go.sum2 --- go.mod | 2 -- go.sum | 15 ++++----------- 2 files changed, 4 insertions(+), 13 deletions(-) diff --git a/go.mod b/go.mod index 54171d0a13a..9949c4ef90c 100644 --- a/go.mod +++ b/go.mod @@ -160,8 +160,6 @@ require ( github.com/bufbuild/protoyaml-go v0.1.8 // indirect github.com/butuzov/ireturn v0.3.0 // indirect github.com/butuzov/mirror v1.2.0 // indirect - github.com/bytedance/sonic v1.12.2 // indirect - github.com/bytedance/sonic/loader v0.2.1 // indirect github.com/campoy/embedmd v1.0.0 // indirect github.com/catenacyber/perfsprint v0.7.1 // indirect github.com/catppuccin/go v0.2.0 // indirect diff --git a/go.sum b/go.sum index 56a8f80eeb6..e28191b5e46 100644 --- a/go.sum +++ b/go.sum @@ -216,10 +216,8 @@ github.com/butuzov/ireturn v0.3.0 h1:hTjMqWw3y5JC3kpnC5vXmFJAWI/m31jaCYQqzkS6PL0 github.com/butuzov/ireturn v0.3.0/go.mod h1:A09nIiwiqzN/IoVo9ogpa0Hzi9fex1kd9PSD6edP5ZA= github.com/butuzov/mirror v1.2.0 h1:9YVK1qIjNspaqWutSv8gsge2e/Xpq1eqEkslEUHy5cs= github.com/butuzov/mirror v1.2.0/go.mod h1:DqZZDtzm42wIAIyHXeN8W/qb1EPlb9Qn/if9icBOpdQ= -github.com/bytedance/sonic v1.12.2 h1:oaMFuRTpMHYLpCntGca65YWt5ny+wAceDERTkT2L9lg= -github.com/bytedance/sonic v1.12.2/go.mod h1:B8Gt/XvtZ3Fqj+iSKMypzymZxw/FVwgIGKzMzT9r/rk= -github.com/bytedance/sonic/loader v0.2.1 h1:1GgorWTqf12TA8mma4DDSbaQigE2wOgQo7iCjjJv3+E= -github.com/bytedance/sonic/loader v0.2.1/go.mod h1:ncP89zfokxS5LZrJxl5z0UJcsk4M4yY2JpfqGeCtNLU= +github.com/bytedance/sonic v1.9.1 h1:6iJ6NqdoxCDr6mbY8h18oSO+cShGSMRGCEo7F2h0x8s= +github.com/bytedance/sonic v1.9.1/go.mod h1:i736AoUSYt75HyZLoJW9ERYxcy6eaN6h4BZXU064P/U= github.com/campoy/embedmd v1.0.0 h1:V4kI2qTJJLf4J29RzI/MAt2c3Bl4dQSYPuflzwFH2hY= github.com/campoy/embedmd v1.0.0/go.mod h1:oxyr9RCiSXg0M3VJ3ks0UGfp98BpSSGr0kpiX3MzVl8= github.com/casbin/casbin/v2 v2.1.2/go.mod h1:YcPU1XXisHhLzuxH9coDNf2FbKpjGlbCg3n9yuLkIJQ= @@ -263,6 +261,8 @@ github.com/charmbracelet/x/term v0.2.0 h1:cNB9Ot9q8I711MyZ7myUR5HFWL/lc3OpU8jZ4h github.com/charmbracelet/x/term v0.2.0/go.mod h1:GVxgxAbjUrmpvIINHIQnJJKpMlHiZ4cktEQCN6GWyF0= github.com/chavacava/garif v0.1.0 h1:2JHa3hbYf5D9dsgseMKAmc/MZ109otzgNFk5s87H9Pc= github.com/chavacava/garif v0.1.0/go.mod h1:XMyYCkEL58DF0oyW4qDjjnPWONs2HBqYKI+UIPD+Gww= +github.com/chenzhuoyu/base64x v0.0.0-20221115062448-fe3a3abad311 h1:qSGYFH7+jGhDF8vLC+iwCD4WpbV1EBDSzWkJODFLams= +github.com/chenzhuoyu/base64x v0.0.0-20221115062448-fe3a3abad311/go.mod h1:b583jCggY9gE99b6G5LEC39OIiVsWj+R97kbl5odCEk= github.com/chewxy/hm v1.0.0 h1:zy/TSv3LV2nD3dwUEQL2VhXeoXbb9QkpmdRAVUFiA6k= github.com/chewxy/hm v1.0.0/go.mod h1:qg9YI4q6Fkj/whwHR1D+bOGeF7SniIP40VweVepLjg0= github.com/chewxy/math32 v1.0.0/go.mod h1:Miac6hA1ohdDUTagnvJy/q+aNnEk16qWUdb8ZVhvCN0= @@ -281,10 +281,6 @@ github.com/ckaznocha/intrange v0.2.0 h1:FykcZuJ8BD7oX93YbO1UY9oZtkRbp+1/kJcDjkef github.com/ckaznocha/intrange v0.2.0/go.mod h1:r5I7nUlAAG56xmkOpw4XVr16BXhwYTUdcuRFeevn1oE= github.com/clbanning/x2j v0.0.0-20191024224557-825249438eec/go.mod h1:jMjuTZXRI4dUb/I5gc9Hdhagfvm9+RyrPryS/auMzxE= github.com/client9/misspell v0.3.4/go.mod h1:qj6jICC3Q7zFZvVWo7KLAzC3yx5G7kyvSDkc90ppPyw= -github.com/cloudwego/base64x v0.1.4 h1:jwCgWpFanWmN8xoIUHa2rtzmkd5J2plF/dnLS6Xd/0Y= -github.com/cloudwego/base64x v0.1.4/go.mod h1:0zlkT4Wn5C6NdauXdJRhSKRlJvmclQ1hhJgA0rcu/8w= -github.com/cloudwego/iasm v0.2.0 h1:1KNIy1I1H9hNNFEEH3DVnI4UujN+1zjpuk6gwHLTssg= -github.com/cloudwego/iasm v0.2.0/go.mod h1:8rXZaNYT2n95jn+zTI1sDr+IgcD2GVs0nlbbQPiEFhY= github.com/cncf/udpa/go v0.0.0-20191209042840-269d4d468f6f/go.mod h1:M8M6+tZqaGXZJjfX53e64911xZQV5JYwmTeXPW+k8Sc= github.com/cncf/udpa/go v0.0.0-20201120205902-5459f2c99403/go.mod h1:WmhPx2Nbnhtbo57+VJT5O0JRkEi1Wbu0z5j0R8u5Hbk= github.com/cncf/udpa/go v0.0.0-20210930031921-04548b0d99d4/go.mod h1:6pvJx4me5XPnfI9Z40ddWsdw2W/uZgQLFXToKeRcDiI= @@ -851,12 +847,10 @@ github.com/klauspost/compress v1.17.7 h1:ehO88t2UGzQK66LMdE8tibEd1ErmzZjNEqWkjLA github.com/klauspost/compress v1.17.7/go.mod h1:Di0epgTjJY877eYKx5yC51cX2A2Vl2ibi7bDH9ttBbw= github.com/klauspost/cpuid v0.0.0-20180405133222-e7e905edc00e h1:+lIPJOWl+jSiJOc70QXJ07+2eg2Jy2EC7Mi11BWujeM= github.com/klauspost/cpuid v0.0.0-20180405133222-e7e905edc00e/go.mod h1:Pj4uuM528wm8OyEC2QMXAi2YiTZ96dNQPGgoMS4s3ek= -github.com/klauspost/cpuid/v2 v2.0.9/go.mod h1:FInQzS24/EEf25PyTYn52gqo7WaD8xa0213Md/qVLRg= github.com/klauspost/cpuid/v2 v2.2.4 h1:acbojRNwl3o09bUq+yDCtZFc1aiwaAAxtcn8YkZXnvk= github.com/klauspost/cpuid/v2 v2.2.4/go.mod h1:RVVoqg1df56z8g3pUjL/3lE5UfnlrJX8tyFgg4nqhuY= github.com/klauspost/pgzip v1.2.6 h1:8RXeL5crjEUFnR2/Sn6GJNWtSQ3Dk8pq4CL3jvdDyjU= github.com/klauspost/pgzip v1.2.6/go.mod h1:Ch1tH69qFZu15pkjo5kYi6mth2Zzwzt50oCQKQE9RUs= -github.com/knz/go-libedit v1.10.1/go.mod h1:MZTVkCWyz0oBc7JOWP3wNAzd002ZbM/5hgShxwh4x8M= github.com/konsorten/go-windows-terminal-sequences v1.0.1/go.mod h1:T0+1ngSBFLxvqU3pZ+m/2kptfBszLMUkC4ZK/EgS/cQ= github.com/konsorten/go-windows-terminal-sequences v1.0.3/go.mod h1:T0+1ngSBFLxvqU3pZ+m/2kptfBszLMUkC4ZK/EgS/cQ= github.com/kr/fs v0.1.0/go.mod h1:FFnZGqtBN9Gxj7eW1uZ42v5BccTP0vu6NEaFoC2HwRg= @@ -2136,7 +2130,6 @@ mvdan.cc/unparam v0.0.0-20240528143540-8a5130ca722f/go.mod h1:RSLa7mKKCNeTTMHBw5 nhooyr.io/websocket v1.8.6/go.mod h1:B70DZP8IakI65RVQ51MsWP/8jndNma26DVA/nFSCgW0= nhooyr.io/websocket v1.8.7 h1:usjR2uOr/zjjkVMy0lW+PPohFok7PCow5sDjLgX4P4g= nhooyr.io/websocket v1.8.7/go.mod h1:B70DZP8IakI65RVQ51MsWP/8jndNma26DVA/nFSCgW0= -nullprogram.com/x/optparse v1.0.0/go.mod h1:KdyPE+Igbe0jQUrVfMqDMeJQIJZEuyV7pjYmp6pbG50= periph.io/x/conn/v3 v3.7.0 h1:f1EXLn4pkf7AEWwkol2gilCNZ0ElY+bxS4WE2PQXfrA= periph.io/x/conn/v3 v3.7.0/go.mod h1:ypY7UVxgDbP9PJGwFSVelRRagxyXYfttVh7hJZUHEhg= periph.io/x/host/v3 v3.8.1-0.20230331112814-9f0d9f7d76db h1:8+HL7DJFofYRhGoK/UdwhzvQj3I2HrKLQ6dkOC66CZY= From ee5b1720b9ff761dab6697f706af5b3fd7f01e38 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 22 Nov 2024 11:00:44 -0500 Subject: [PATCH 10/13] remove space --- motionplan/collision_test.go | 1 - 1 file changed, 1 deletion(-) diff --git a/motionplan/collision_test.go b/motionplan/collision_test.go index 13fdd51530f..9c35f7810da 100644 --- a/motionplan/collision_test.go +++ b/motionplan/collision_test.go @@ -73,7 +73,6 @@ func TestCheckCollisions(t *testing.T) { // case 2: zero position of xArm6 arm - should have number of collisions = to number of geometries - 1 // no external geometries considered, self collision only - m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) gf, _ := m.Geometries(make([]frame.Input, len(m.DoF()))) From a72151a52a35f192308e08f8aa92c5ecdd7af7cc Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 22 Nov 2024 11:02:52 -0500 Subject: [PATCH 11/13] update absolute path to not have errors in it --- motionplan/kinematic_test.go | 8 ++++---- motionplan/motionPlanner_test.go | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/motionplan/kinematic_test.go b/motionplan/kinematic_test.go index e5ed5f24b5e..e7c7941e60e 100644 --- a/motionplan/kinematic_test.go +++ b/motionplan/kinematic_test.go @@ -41,7 +41,7 @@ func TestForwardKinematics(t *testing.T) { test.That(t, spatial.PoseAlmostEqual(expect, pos), test.ShouldBeTrue) // Test the 6dof xarm we actually have - m, err = frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fakexarm6_kinematics_test.json"), "") + m, err = frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) // Confirm end effector starts at 207, 0, 112 @@ -159,7 +159,7 @@ func TestDeriv(t *testing.T) { func TestDynamicFrameSystemXArm(t *testing.T) { fs := frame.NewEmptyFrameSystem("test") - model, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fakexarm6_kinematics_test.json"), "") + model, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) fs.AddFrame(model, fs.World()) @@ -212,7 +212,7 @@ func TestComplicatedDynamicFrameSystem(t *testing.T) { fs.AddFrame(gantryY, gantryX) // xarm on gantry - modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fakexarm6_kinematics_test.json"), "") + modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) fs.AddFrame(modelXarm, gantryY) @@ -316,7 +316,7 @@ func TestKinematicsJSONvsURDF(t *testing.T) { } func TestComputeOOBPosition(t *testing.T) { - model, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fakexarm6_kinematics_test.json"), "") + model, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) test.That(t, model.Name(), test.ShouldEqual, "foo") diff --git a/motionplan/motionPlanner_test.go b/motionplan/motionPlanner_test.go index 956888cec1c..9aa781166da 100644 --- a/motionplan/motionPlanner_test.go +++ b/motionplan/motionPlanner_test.go @@ -450,7 +450,7 @@ func makeTestFS(t *testing.T) frame.FrameSystem { test.That(t, err, test.ShouldBeNil) fs.AddFrame(gantryY, gantryX) - modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fakexarm6_kinematics_test.json"), "") + modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) fs.AddFrame(modelXarm, gantryY) @@ -584,7 +584,7 @@ func TestMultiArmSolve(t *testing.T) { func TestReachOverArm(t *testing.T) { // setup frame system with an xarm - xarm, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fakexarm6_kinematics_test.json"), "") + xarm, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) offset, err := frame.NewStaticFrame("offset", spatialmath.NewPoseFromPoint(r3.Vector{X: -500, Y: 200})) test.That(t, err, test.ShouldBeNil) From a0ca9f4923ead2bf5c642c77540c94fd3416572a Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 22 Nov 2024 11:18:21 -0500 Subject: [PATCH 12/13] appease linter --- components/arm/fake/fake.go | 1 - 1 file changed, 1 deletion(-) diff --git a/components/arm/fake/fake.go b/components/arm/fake/fake.go index 69e57b37a64..99e1c28c301 100644 --- a/components/arm/fake/fake.go +++ b/components/arm/fake/fake.go @@ -9,7 +9,6 @@ import ( "sync" "github.com/pkg/errors" - // xarm "github.com/viam-modules/viam-ufactory-xarm/arm" "go.viam.com/rdk/components/arm" "go.viam.com/rdk/components/arm/eva" From 99ebeaa53aa3784ee47a3098bd5a0b7ec76173b6 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 22 Nov 2024 12:20:34 -0500 Subject: [PATCH 13/13] the name is 'foo' --- motionplan/kinematic_test.go | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motionplan/kinematic_test.go b/motionplan/kinematic_test.go index e7c7941e60e..a3a9a577e15 100644 --- a/motionplan/kinematic_test.go +++ b/motionplan/kinematic_test.go @@ -316,7 +316,7 @@ func TestKinematicsJSONvsURDF(t *testing.T) { } func TestComputeOOBPosition(t *testing.T) { - model, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm6_kinematics_test.json"), "") + model, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm6_kinematics_test.json"), "foo") test.That(t, err, test.ShouldBeNil) test.That(t, model.Name(), test.ShouldEqual, "foo")