From 4e841560049453d610cb38323e76fd2edcce9b0e Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Mon, 9 Dec 2024 13:01:02 -0500 Subject: [PATCH] delete the Eva arm (#4613) --- components/arm/eva/eva.go | 429 ------------------------- components/arm/eva/eva_kinematics.json | 149 --------- components/arm/fake/fake.go | 3 - components/arm/register/register.go | 1 - referenceframe/model_json_test.go | 1 - 5 files changed, 583 deletions(-) delete mode 100644 components/arm/eva/eva.go delete mode 100644 components/arm/eva/eva_kinematics.json diff --git a/components/arm/eva/eva.go b/components/arm/eva/eva.go deleted file mode 100644 index 8d85334ec7c..00000000000 --- a/components/arm/eva/eva.go +++ /dev/null @@ -1,429 +0,0 @@ -// Package eva implements the Eva robot from Automata. -// api found at -// https://github.com/automata-tech/eva_python_sdk/blob/development/evasdk/eva_http_client.py -package eva - -import ( - "bytes" - "context" - // for embedding model file. - _ "embed" - "encoding/json" - "fmt" - "io" - "net/http" - "strings" - "sync" - "time" - - "github.com/pkg/errors" - "go.uber.org/multierr" - "go.viam.com/utils" - - "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/services/motion" - "go.viam.com/rdk/spatialmath" -) - -// Model is the name of the eva model of an arm component. -var Model = resource.DefaultModelFamily.WithModel("eva") - -// Config is used for converting config attributes. -type Config struct { - resource.TriviallyValidateConfig - Token string `json:"token"` - Host string `json:"host"` -} - -//go:embed eva_kinematics.json -var evamodeljson []byte - -func init() { - resource.RegisterComponent(arm.API, Model, resource.Registration[arm.Arm, *Config]{ - Constructor: func( - ctx context.Context, _ resource.Dependencies, conf resource.Config, logger logging.Logger, - ) (arm.Arm, error) { - return NewEva(ctx, conf, logger) - }, - }) -} - -type evaData struct { - // map[estop:false] - Global map[string]interface{} - - // map[d0:false d1:false d2:false d3:false ee_a0:0.034 ee_a1:0.035 ee_d0:false ee_d1:false] - GlobalInputs map[string]interface{} `json:"global.inputs"` - - // map[d0:false d1:false d2:false d3:false ee_d0:false ee_d1:false] - GlobalOutputs map[string]interface{} `json:"global.outputs"` - - // scheduler : map[enabled:false] - Scheduler map[string]interface{} - - // [0.0008628905634395778 0 0.0002876301878131926 0 -0.00038350690738298 0.0005752603756263852] - ServosPosition []float64 `json:"servos.telemetry.position"` - - // [53.369998931884766 43.75 43.869998931884766 43.869998931884766 51 48.619998931884766] - ServosTemperature []float64 `json:"servos.telemetry.temperature"` - - // [0 0 0 0 0 0] - ServosVelocity []float64 `json:"servos.telemetry.velocity"` - - // map[loop_count:1 loop_target:1 run_mode:not_running state:ready toolpath_hash:4d8 toolpath_name:Uploaded] - Control map[string]interface{} -} - -type eva struct { - resource.Named - resource.AlwaysRebuild - host string - version string - token string - sessionToken string - - moveLock sync.Mutex - logger logging.Logger - model referenceframe.Model - - frameJSON []byte - - opMgr *operation.SingleOperationManager -} - -// NewEva TODO. -func NewEva(ctx context.Context, conf resource.Config, logger logging.Logger) (arm.Arm, error) { - model, err := MakeModelFrame(conf.Name) - if err != nil { - return nil, err - } - - newConf, err := resource.NativeConfig[*Config](conf) - if err != nil { - return nil, err - } - - e := &eva{ - Named: conf.ResourceName().AsNamed(), - host: newConf.Host, - version: "v1", - token: newConf.Token, - logger: logger, - model: model, - frameJSON: evamodeljson, - opMgr: operation.NewSingleOperationManager(), - } - - name, err := e.apiName(ctx) - if err != nil { - return nil, err - } - - e.logger.CDebugf(ctx, "connected to eva: %v", name) - - return e, nil -} - -func (e *eva) JointPositions(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) { - data, err := e.DataSnapshot(ctx) - if err != nil { - return nil, err - } - return referenceframe.FloatsToInputs(data.ServosPosition), nil -} - -// EndPosition computes and returns the current cartesian position. -func (e *eva) EndPosition(ctx context.Context, extra map[string]interface{}) (spatialmath.Pose, error) { - joints, err := e.CurrentInputs(ctx) - if err != nil { - return nil, err - } - return referenceframe.ComputeOOBPosition(e.model, joints) -} - -// MoveToPosition moves the arm to the specified cartesian position. -func (e *eva) MoveToPosition(ctx context.Context, pos spatialmath.Pose, extra map[string]interface{}) error { - ctx, done := e.opMgr.New(ctx) - defer done() - return motion.MoveArm(ctx, e.logger, e, pos) -} - -func (e *eva) MoveToJointPositions(ctx context.Context, newPositions []referenceframe.Input, extra map[string]interface{}) error { - // check that joint positions are not out of bounds - if err := arm.CheckDesiredJointPositions(ctx, e, newPositions); err != nil { - return err - } - ctx, done := e.opMgr.New(ctx) - defer done() - - radians := referenceframe.InputsToFloats(newPositions) - err := e.doMoveJoints(ctx, radians) - if err == nil { - return nil - } - - if !strings.Contains(err.Error(), "Reset hard errors first") { - return err - } - - if err2 := e.resetErrors(ctx); err2 != nil { - return errors.Wrapf(multierr.Combine(err, err2), "move failure, and couldn't reset errors") - } - - return e.doMoveJoints(ctx, radians) -} - -func (e *eva) MoveThroughJointPositions( - ctx context.Context, - positions [][]referenceframe.Input, - _ *arm.MoveOptions, - _ map[string]interface{}, -) error { - for _, goal := range positions { - if err := arm.CheckDesiredJointPositions(ctx, e, goal); err != nil { - return err - } - err := e.MoveToJointPositions(ctx, goal, nil) - if err != nil { - return err - } - } - return nil -} - -func (e *eva) doMoveJoints(ctx context.Context, joints []float64) error { - if err := e.apiLock(ctx); err != nil { - return err - } - defer e.apiUnlock(ctx) - - return e.apiControlGoTo(ctx, joints) -} - -func (e *eva) apiRequest(ctx context.Context, method, path string, payload interface{}, auth bool, out interface{}) error { - return e.apiRequestRetry(ctx, method, path, payload, auth, out, true) -} - -func (e *eva) apiRequestRetry( - ctx context.Context, - method string, - path string, - payload interface{}, - auth bool, - out interface{}, - retry bool, -) error { - fullPath := fmt.Sprintf("http://%s/api/%s/%s", e.host, e.version, path) - - var reqReader io.Reader - if payload != nil { - data, err := json.Marshal(payload) - if err != nil { - return err - } - reqReader = bytes.NewReader(data) - } - - req, err := http.NewRequest(method, fullPath, reqReader) - if err != nil { - return err - } - req = req.WithContext(ctx) - - if auth { - req.Header.Add("Authorization", fmt.Sprintf("Bearer %s", e.sessionToken)) - } - - res, err := http.DefaultClient.Do(req) - if err != nil { - return err - } - defer func() { - utils.UncheckedError(res.Body.Close()) - }() - - if res.StatusCode == http.StatusUnauthorized { - // need to login - - if !retry { - return errors.New("got 401 from eva after trying to login") - } - - type Temp struct { - Token string - } - t := Temp{} - err = e.apiRequestRetry(ctx, "POST", "auth", map[string]string{"token": e.token}, false, &t, false) - if err != nil { - return err - } - - e.sessionToken = t.Token - return e.apiRequestRetry(ctx, method, path, payload, auth, out, false) - } - - if res.StatusCode != http.StatusOK { - more := "" - if res.Body != nil { - more2, e2 := io.ReadAll(res.Body) - if e2 == nil { - more = string(more2) - } - } - - return errors.Errorf("got unexpected response code: %d for %s %s", res.StatusCode, fullPath, more) - } - - if out == nil { - return nil - } - - if !strings.HasPrefix(res.Header["Content-Type"][0], "application/json") { - return errors.Errorf("expected json response from eva, got: %v", res.Header["Content-Type"]) - } - - decoder := json.NewDecoder(res.Body) - - return decoder.Decode(out) -} - -func (e *eva) apiName(ctx context.Context) (string, error) { - type Temp struct { - Name string - } - t := Temp{} - - err := e.apiRequest(ctx, "GET", "name", nil, false, &t) - if err != nil { - return "", err - } - - return t.Name, nil -} - -func (e *eva) resetErrors(ctx context.Context) error { - e.moveLock.Lock() - defer e.moveLock.Unlock() - - err := e.apiLock(ctx) - if err != nil { - return err - } - defer e.apiUnlock(ctx) - - err = e.apiRequest(ctx, "POST", "controls/reset_errors", nil, true, nil) - if err != nil { - return err - } - utils.SelectContextOrWait(ctx, 100*time.Millisecond) - return ctx.Err() -} - -func (e *eva) stop(ctx context.Context) error { - if e.opMgr.OpRunning() { - return multierr.Combine( - e.apiRequest(ctx, "POST", "controls/pause", nil, true, nil), // pause robot - e.apiRequest(ctx, "POST", "controls/cancel", nil, true, nil), // make state ready to run again - ) - } - return nil -} - -func (e *eva) Stop(ctx context.Context, extra map[string]interface{}) error { - return e.stop(ctx) -} - -func (e *eva) IsMoving(ctx context.Context) (bool, error) { - return e.opMgr.OpRunning(), nil -} - -func (e *eva) DataSnapshot(ctx context.Context) (evaData, error) { - type Temp struct { - Snapshot evaData - } - res := Temp{} - - err := e.apiRequest(ctx, "GET", "data/snapshot", nil, true, &res) - return res.Snapshot, err -} - -func (e *eva) apiControlGoTo(ctx context.Context, joints []float64) error { - body := map[string]interface{}{"joints": joints, "mode": "teach"} // TODO(erh): change to automatic - err := e.apiRequest(ctx, "POST", "controls/go_to", &body, true, nil) - if err != nil { - return err - } - - // we have to poll till we're done to unlock safely - return e.loopTillNotRunning(ctx) -} - -func (e *eva) loopTillNotRunning(ctx context.Context) error { - for { - data, err := e.DataSnapshot(ctx) - if err != nil { - return err - } - - if data.Control["run_mode"] == "not_running" { - break - } - - if !utils.SelectContextOrWait(ctx, 10*time.Millisecond) { - return ctx.Err() - } - } - - return nil -} - -func (e *eva) apiLock(ctx context.Context) error { - return e.apiRequest(ctx, "POST", "controls/lock", nil, true, nil) -} - -func (e *eva) apiUnlock(ctx context.Context) { - err := e.apiRequest(ctx, "DELETE", "controls/lock", nil, true, nil) - if err != nil { - e.logger.CDebugf(ctx, "eva unlock failed: %s", err) - } -} - -// ModelFrame returns all the information necessary for including the arm in a FrameSystem. -func (e *eva) ModelFrame() referenceframe.Model { - return e.model -} - -func (e *eva) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { - return e.JointPositions(ctx, nil) -} - -func (e *eva) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Input) error { - return e.MoveThroughJointPositions(ctx, inputSteps, nil, nil) -} - -func (e *eva) Close(ctx context.Context) error { - return e.stop(ctx) -} - -// MakeModelFrame returns the kinematics model of the eva arm, also has all Frame information. -func MakeModelFrame(name string) (referenceframe.Model, error) { - return referenceframe.UnmarshalModelJSON(evamodeljson, name) -} - -// Geometries returns the list of geometries associated with the resource, in any order. The poses of the geometries reflect their -// current location relative to the frame of the resource. -func (e *eva) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) { - inputs, err := e.CurrentInputs(ctx) - if err != nil { - return nil, err - } - gif, err := e.model.Geometries(inputs) - if err != nil { - return nil, err - } - return gif.Geometries(), nil -} diff --git a/components/arm/eva/eva_kinematics.json b/components/arm/eva/eva_kinematics.json deleted file mode 100644 index 1d05dd7eea6..00000000000 --- a/components/arm/eva/eva_kinematics.json +++ /dev/null @@ -1,149 +0,0 @@ -{ - "name": "Eva", - "links": [ - { - "id": "base", - "parent": "world", - "translation": { - "x": 0, - "y": 0, - "z": 0 - } - }, - { - "id": "base_top", - "parent": "waist", - "translation": { - "x": 56, - "y": 0, - "z": 283 - } - }, - { - "id": "upper_arm", - "parent": "shoulder", - "translation": { - "x": 0, - "y": 0, - "z": 205 - } - }, - { - "id": "upper_forearm", - "parent": "elbow", - "translation": { - "x": -56, - "y": 0, - "z": 124 - } - }, - { - "id": "lower_forearm", - "parent": "forearm_rot", - "translation": { - "x": 0, - "y": 0, - "z": 167 - } - }, - { - "id": "gripper_mount_base", - "parent": "wrist", - "translation": { - "x": -65, - "y": 0, - "z": 104 - } - }, - { - "id": "gripper_mount", - "parent": "gripper_rot", - "translation": { - "x": 0, - "y": 0, - "z": 0 - }, - "setorientation": true, - "orientation": { - "x": 0, - "y": 0, - "z": 1, - "th": 90 - } - } - ], - "joints": [ - { - "id": "waist", - "type": "revolute", - "parent": "base", - "axis": { - "x": 0, - "y": 0, - "z": 1 - }, - "max": 179, - "min": -179 - }, - { - "id": "shoulder", - "type": "revolute", - "parent": "base_top", - "axis": { - "x": 0, - "y": -1, - "z": 0 - }, - "max": 70, - "min": -155 - }, - { - "id": "elbow", - "type": "revolute", - "parent": "upper_arm", - "axis": { - "x": 0, - "y": -1, - "z": 0 - }, - "max": 45, - "min": -160 - }, - { - "id": "forearm_rot", - "type": "revolute", - "parent": "upper_forearm", - "axis": { - "x": 0, - "y": 0, - "z": 1 - }, - "max": 179, - "min": -179 - }, - { - "id": "wrist", - "type": "revolute", - "parent": "lower_forearm", - "axis": { - "x": 0, - "y": -1, - "z": 0 - }, - "max": 10, - "min": -155 - }, - { - "id": "gripper_rot", - "type": "revolute", - "parent": "gripper_mount_base", - "axis": { - "x": 0, - "y": 0, - "z": 1 - }, - "max": 179, - "min": -179 - } - ] -} diff --git a/components/arm/fake/fake.go b/components/arm/fake/fake.go index f6ab47bb580..4966ef2c096 100644 --- a/components/arm/fake/fake.go +++ b/components/arm/fake/fake.go @@ -10,7 +10,6 @@ import ( "github.com/pkg/errors" "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/logging" "go.viam.com/rdk/motionplan" @@ -257,8 +256,6 @@ func modelFromName(model, name string) (referenceframe.Model, error) { switch model { case ur.Model.Name: return ur.MakeModelFrame(name) - case eva.Model.Name: - return eva.MakeModelFrame(name) case dofbotModel: return referenceframe.UnmarshalModelJSON(dofbotjson, name) case Model.Name: diff --git a/components/arm/register/register.go b/components/arm/register/register.go index a566fca2b63..e7fffbd098a 100644 --- a/components/arm/register/register.go +++ b/components/arm/register/register.go @@ -3,7 +3,6 @@ package register import ( // register arms. - _ "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" diff --git a/referenceframe/model_json_test.go b/referenceframe/model_json_test.go index 9e8fd562079..814e4f91c21 100644 --- a/referenceframe/model_json_test.go +++ b/referenceframe/model_json_test.go @@ -14,7 +14,6 @@ import ( // So we'll just check that we read in the right number of joints. func TestParseJSONFile(t *testing.T) { goodFiles := []string{ - "components/arm/eva/eva_kinematics.json", "components/arm/example_kinematics/xarm6_kinematics_test.json", "components/arm/example_kinematics/xarm7_kinematics_test.json", "referenceframe/testjson/ur5eDH.json",