diff --git a/components/arm/fake/fake.go b/components/arm/fake/fake.go index 92947835cf1..99e1c28c301 100644 --- a/components/arm/fake/fake.go +++ b/components/arm/fake/fake.go @@ -4,6 +4,7 @@ package fake import ( "context" _ "embed" + "fmt" "strings" "sync" @@ -12,7 +13,6 @@ import ( "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" @@ -35,6 +35,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 +271,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 +296,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/xarm/lite6_kinematics.json b/components/arm/fake/lite6_kinematics_test.json similarity index 100% rename from components/arm/xarm/lite6_kinematics.json rename to components/arm/fake/lite6_kinematics_test.json diff --git a/components/arm/xarm/xarm6_kinematics.json b/components/arm/fake/xarm6_kinematics_test.json similarity index 100% rename from components/arm/xarm/xarm6_kinematics.json rename to components/arm/fake/xarm6_kinematics_test.json diff --git a/components/arm/xarm/xarm7_kinematics.json b/components/arm/fake/xarm7_kinematics_test.json similarity index 100% rename from components/arm/xarm/xarm7_kinematics.json rename to components/arm/fake/xarm7_kinematics_test.json diff --git a/components/arm/register/register.go b/components/arm/register/register.go index 23607085676..a566fca2b63 100644 --- a/components/arm/register/register.go +++ b/components/arm/register/register.go @@ -7,5 +7,4 @@ import ( _ "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/wrapper/wrapper_test.go b/components/arm/wrapper/wrapper_test.go index b0d9bbd11af..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: "../xarm/xarm6_kinematics.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: "../xarm/xarm6_kinematics.json", + ModelFilePath: "../fake/xarm6_kinematics_test.json", ArmName: "dne1", }, } 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/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/motionplan/cBiRRT_test.go b/motionplan/cBiRRT_test.go index 5378471cf47..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("components/arm/xarm/xarm7_kinematics.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 b32fbffbfb4..9c35f7810da 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("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 +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("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 aba13c99398..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("components/arm/xarm/xarm6_kinematics.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("components/arm/xarm/xarm7_kinematics.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("components/arm/xarm/xarm6_kinematics.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("components/arm/xarm/xarm6_kinematics.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 c3157f3b0ef..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("components/arm/xarm/xarm6_kinematics.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 536852604b9..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("components/arm/xarm/xarm6_kinematics.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("components/arm/xarm/xarm7_kinematics.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 a6e50e2448c..a3a9a577e15 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("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("components/arm/xarm/xarm6_kinematics.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/xarm/xarm6_kinematics.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/xarm/xarm6_kinematics.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/xarm/xarm6_kinematics.json"), "foo") + 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") diff --git a/motionplan/motionPlanner_test.go b/motionplan/motionPlanner_test.go index 0b1c8fda434..9aa781166da 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("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("components/arm/xarm/xarm7_kinematics.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("components/arm/xarm/xarm6_kinematics.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/xarm/xarm6_kinematics.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) @@ -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("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("components/arm/xarm/xarm6_kinematics.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 2430c261a98..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", - "components/arm/xarm/xarm6_kinematics.json", - "components/arm/xarm/xarm7_kinematics.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 6bda57de026..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("components/arm/xarm/xarm6_kinematics.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("components/arm/xarm/xarm6_kinematics.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("components/arm/xarm/xarm6_kinematics.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("components/arm/xarm/xarm6_kinematics.json"), "") + m, err := ParseModelJSONFile(utils.ResolveFile("components/arm/fake/xarm6_kinematics_test.json"), "") test.That(t, err, test.ShouldBeNil) dof := len(m.DoF())