Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

RSDK-223 - support translational joints for arm components #739

Closed
wants to merge 2 commits into from
Closed

RSDK-223 - support translational joints for arm components #739

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
916bbf8
RSDK-223 - support translational joints for arm components
gvaradarajan Apr 18, 2022
73d0fbb
lint
gvaradarajan Apr 21, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 10 additions & 6 deletions component/arm/arm.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,10 +76,10 @@ type Arm interface {
MoveToPosition(ctx context.Context, pose *commonpb.Pose, worldState *commonpb.WorldState) error

// MoveToJointPositions moves the arm's joints to the given positions.
MoveToJointPositions(ctx context.Context, positionDegs *pb.JointPositions) error
MoveToJointPositions(ctx context.Context, jointPositions []*pb.JointPosition) error

// GetJointPositions returns the current joint positions of the arm.
GetJointPositions(ctx context.Context) (*pb.JointPositions, error)
GetJointPositions(ctx context.Context) ([]*pb.JointPosition, error)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think this api needs discussion not in a PR.


referenceframe.ModelFramer
referenceframe.InputEnabled
Expand Down Expand Up @@ -122,8 +122,12 @@ func CreateStatus(ctx context.Context, resource interface{}) (*pb.Status, error)
if err != nil {
return nil, err
}
jointPositionRawValues := make([]float64, len(jointPositions))
for idx, jp := range jointPositions {
jointPositionRawValues[idx] = jp.GetParameters()[0]
}

return &pb.Status{EndPosition: endPosition, JointPositions: jointPositions}, nil
return &pb.Status{EndPosition: endPosition, JointPositions: &pb.JointPositions{Degrees: jointPositionRawValues}}, nil
}

type reconfigurableArm struct {
Expand All @@ -149,13 +153,13 @@ func (r *reconfigurableArm) MoveToPosition(ctx context.Context, pose *commonpb.P
return r.actual.MoveToPosition(ctx, pose, worldState)
}

func (r *reconfigurableArm) MoveToJointPositions(ctx context.Context, positionDegs *pb.JointPositions) error {
func (r *reconfigurableArm) MoveToJointPositions(ctx context.Context, jointPositions []*pb.JointPosition) error {
r.mu.RLock()
defer r.mu.RUnlock()
return r.actual.MoveToJointPositions(ctx, positionDegs)
return r.actual.MoveToJointPositions(ctx, jointPositions)
}

func (r *reconfigurableArm) GetJointPositions(ctx context.Context) (*pb.JointPositions, error) {
func (r *reconfigurableArm) GetJointPositions(ctx context.Context) ([]*pb.JointPosition, error) {
r.mu.RLock()
defer r.mu.RUnlock()
return r.actual.GetJointPositions(ctx)
Expand Down
13 changes: 10 additions & 3 deletions component/arm/arm_test.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -115,8 +115,15 @@ func TestCreateStatus(t *testing.T) {
injectArm.GetEndPositionFunc = func(ctx context.Context) (*commonpb.Pose, error) {
return pose, nil
}
injectArm.GetJointPositionsFunc = func(ctx context.Context) (*pb.JointPositions, error) {
return &pb.JointPositions{Degrees: status.JointPositions.Degrees}, nil
injectArm.GetJointPositionsFunc = func(ctx context.Context) ([]*pb.JointPosition, error) {
result := make([]*pb.JointPosition, len(status.JointPositions.Degrees))
for i, deg := range status.JointPositions.Degrees {
result[i] = &pb.JointPosition{
JointType: pb.JointPosition_JOINT_TYPE_REVOLUTE,
Parameters: []float64{deg},
}
}
return result, nil
}

t.Run("working", func(t *testing.T) {
Expand All @@ -127,7 +134,7 @@ func TestCreateStatus(t *testing.T) {

t.Run("fail on GetJointPositions", func(t *testing.T) {
errFail := errors.New("can't get joint positions")
injectArm.GetJointPositionsFunc = func(ctx context.Context) (*pb.JointPositions, error) {
injectArm.GetJointPositionsFunc = func(ctx context.Context) ([]*pb.JointPosition, error) {
return nil, errFail
}
_, err = arm.CreateStatus(context.Background(), injectArm)
Expand Down
12 changes: 6 additions & 6 deletions component/arm/client.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -90,22 +90,22 @@ func (c *client) MoveToPosition(ctx context.Context, pose *commonpb.Pose, worldS
return err
}

func (c *client) MoveToJointPositions(ctx context.Context, positionDegs *pb.JointPositions) error {
func (c *client) MoveToJointPositions(ctx context.Context, jointPositions []*pb.JointPosition) error {
_, err := c.client.MoveToJointPositions(ctx, &pb.MoveToJointPositionsRequest{
Name: c.name,
PositionDegs: positionDegs,
Name: c.name,
JointPositions: jointPositions,
})
return err
}

func (c *client) GetJointPositions(ctx context.Context) (*pb.JointPositions, error) {
func (c *client) GetJointPositions(ctx context.Context) ([]*pb.JointPosition, error) {
resp, err := c.client.GetJointPositions(ctx, &pb.GetJointPositionsRequest{
Name: c.name,
})
if err != nil {
return nil, err
}
return resp.PositionDegs, nil
return resp.GetJointPositions(), nil
}

func (c *client) ModelFrame() referenceframe.Model {
Expand All @@ -118,7 +118,7 @@ func (c *client) CurrentInputs(ctx context.Context) ([]referenceframe.Input, err
if err != nil {
return nil, err
}
return referenceframe.JointPosToInputs(res), nil
return referenceframe.JointPosToInputs(res)
}

func (c *client) GoToInputs(ctx context.Context, goal []referenceframe.Input) error {
Expand Down
51 changes: 42 additions & 9 deletions component/arm/client_test.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,43 +31,70 @@ func TestClient(t *testing.T) {

var (
capArmPos *commonpb.Pose
capArmJointPos *componentpb.JointPositions
capArmJointPos []*componentpb.JointPosition
)

pos1 := &commonpb.Pose{X: 1, Y: 2, Z: 3}
jointPos1 := &componentpb.JointPositions{Degrees: []float64{1.0, 2.0, 3.0}}
// jointPos1 := &componentpb.JointPositions{Degrees: []float64{1.0, 2.0, 3.0}}
jointPos1 := []*componentpb.JointPosition{
{
JointType: componentpb.JointPosition_JOINT_TYPE_REVOLUTE,
Parameters: []float64{1.0},
},
{
JointType: componentpb.JointPosition_JOINT_TYPE_REVOLUTE,
Parameters: []float64{2.0},
},
{
JointType: componentpb.JointPosition_JOINT_TYPE_REVOLUTE,
Parameters: []float64{3.0},
},
}
injectArm := &inject.Arm{}
injectArm.GetEndPositionFunc = func(ctx context.Context) (*commonpb.Pose, error) {
return pos1, nil
}
injectArm.GetJointPositionsFunc = func(ctx context.Context) (*componentpb.JointPositions, error) {
injectArm.GetJointPositionsFunc = func(ctx context.Context) ([]*componentpb.JointPosition, error) {
return jointPos1, nil
}
injectArm.MoveToPositionFunc = func(ctx context.Context, ap *commonpb.Pose, worldState *commonpb.WorldState) error {
capArmPos = ap
return nil
}

injectArm.MoveToJointPositionsFunc = func(ctx context.Context, jp *componentpb.JointPositions) error {
injectArm.MoveToJointPositionsFunc = func(ctx context.Context, jp []*componentpb.JointPosition) error {
capArmJointPos = jp
return nil
}

pos2 := &commonpb.Pose{X: 4, Y: 5, Z: 6}
jointPos2 := &componentpb.JointPositions{Degrees: []float64{4.0, 5.0, 6.0}}
jointPos2 := []*componentpb.JointPosition{
{
JointType: componentpb.JointPosition_JOINT_TYPE_REVOLUTE,
Parameters: []float64{1.0},
},
{
JointType: componentpb.JointPosition_JOINT_TYPE_REVOLUTE,
Parameters: []float64{2.0},
},
{
JointType: componentpb.JointPosition_JOINT_TYPE_REVOLUTE,
Parameters: []float64{3.0},
},
}
injectArm2 := &inject.Arm{}
injectArm2.GetEndPositionFunc = func(ctx context.Context) (*commonpb.Pose, error) {
return pos2, nil
}
injectArm2.GetJointPositionsFunc = func(ctx context.Context) (*componentpb.JointPositions, error) {
injectArm2.GetJointPositionsFunc = func(ctx context.Context) ([]*componentpb.JointPosition, error) {
return jointPos2, nil
}
injectArm2.MoveToPositionFunc = func(ctx context.Context, ap *commonpb.Pose, worldState *commonpb.WorldState) error {
capArmPos = ap
return nil
}

injectArm2.MoveToJointPositionsFunc = func(ctx context.Context, jp *componentpb.JointPositions) error {
injectArm2.MoveToJointPositionsFunc = func(ctx context.Context, jp []*componentpb.JointPosition) error {
capArmJointPos = jp
return nil
}
Expand Down Expand Up @@ -100,15 +127,21 @@ func TestClient(t *testing.T) {

jointPos, err := arm1Client.GetJointPositions(context.Background())
test.That(t, err, test.ShouldBeNil)
test.That(t, jointPos.String(), test.ShouldResemble, jointPos1.String())
for idx, jp := range jointPos {
test.That(t, jp.JointType, test.ShouldEqual, jointPos1[idx].JointType)
test.That(t, jp.GetParameters(), test.ShouldResemble, jointPos1[idx].GetParameters())
}

err = arm1Client.MoveToPosition(context.Background(), pos2, &commonpb.WorldState{})
test.That(t, err, test.ShouldBeNil)
test.That(t, capArmPos.String(), test.ShouldResemble, pos2.String())

err = arm1Client.MoveToJointPositions(context.Background(), jointPos2)
test.That(t, err, test.ShouldBeNil)
test.That(t, capArmJointPos.String(), test.ShouldResemble, jointPos2.String())
for idx, jp := range capArmJointPos {
test.That(t, jp.JointType, test.ShouldEqual, jointPos2[idx].JointType)
test.That(t, jp.GetParameters(), test.ShouldResemble, jointPos2[idx].GetParameters())
}

test.That(t, utils.TryClose(context.Background(), arm1Client), test.ShouldBeNil)
})
Expand Down
14 changes: 9 additions & 5 deletions component/arm/eva/eva.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,10 +99,10 @@ type eva struct {
frameJSON []byte
}

func (e *eva) GetJointPositions(ctx context.Context) (*pb.JointPositions, error) {
func (e *eva) GetJointPositions(ctx context.Context) ([]*pb.JointPosition, error) {
data, err := e.DataSnapshot(ctx)
if err != nil {
return &pb.JointPositions{}, err
return nil, err
}
return referenceframe.JointPositionsFromRadians(data.ServosPosition), nil
}
Expand All @@ -122,14 +122,18 @@ func (e *eva) MoveToPosition(ctx context.Context, pos *commonpb.Pose, worldState
if err != nil {
return err
}
solution, err := e.mp.Plan(ctx, pos, referenceframe.JointPosToInputs(joints), nil)
inputs, err := referenceframe.JointPosToInputs(joints)
if err != nil {
return err
}
solution, err := e.mp.Plan(ctx, pos, inputs, nil)
if err != nil {
return err
}
return arm.GoToWaypoints(ctx, e, solution)
}

func (e *eva) MoveToJointPositions(ctx context.Context, newPositions *pb.JointPositions) error {
func (e *eva) MoveToJointPositions(ctx context.Context, newPositions []*pb.JointPosition) error {
radians := referenceframe.JointPositionsToRadians(newPositions)

err := e.doMoveJoints(ctx, radians)
Expand Down Expand Up @@ -337,7 +341,7 @@ func (e *eva) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error)
if err != nil {
return nil, err
}
return referenceframe.JointPosToInputs(res), nil
return referenceframe.JointPosToInputs(res)
}

func (e *eva) GoToInputs(ctx context.Context, goal []referenceframe.Input) error {
Expand Down
17 changes: 12 additions & 5 deletions component/arm/fake/arm.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,10 +40,17 @@ func NewArm(cfg config.Component) (arm.Arm, error) {
if err != nil {
return nil, err
}
jointPositions := make([]*pb.JointPosition, 6)
for i := 0; i < 6; i++ {
jointPositions[i] = &pb.JointPosition{
JointType: pb.JointPosition_JOINT_TYPE_REVOLUTE,
Parameters: []float64{0.0},
}
}
return &Arm{
Name: name,
position: &commonpb.Pose{},
joints: &pb.JointPositions{Degrees: []float64{0, 0, 0, 0, 0, 0}},
joints: jointPositions,
model: model,
}, nil
}
Expand All @@ -52,7 +59,7 @@ func NewArm(cfg config.Component) (arm.Arm, error) {
type Arm struct {
Name string
position *commonpb.Pose
joints *pb.JointPositions
joints []*pb.JointPosition
CloseCount int
model referenceframe.Model
}
Expand All @@ -74,13 +81,13 @@ func (a *Arm) MoveToPosition(ctx context.Context, c *commonpb.Pose, worldState *
}

// MoveToJointPositions sets the joints.
func (a *Arm) MoveToJointPositions(ctx context.Context, joints *pb.JointPositions) error {
func (a *Arm) MoveToJointPositions(ctx context.Context, joints []*pb.JointPosition) error {
a.joints = joints
return nil
}

// GetJointPositions returns the set joints.
func (a *Arm) GetJointPositions(ctx context.Context) (*pb.JointPositions, error) {
func (a *Arm) GetJointPositions(ctx context.Context) ([]*pb.JointPosition, error) {
return a.joints, nil
}

Expand All @@ -90,7 +97,7 @@ func (a *Arm) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error)
if err != nil {
return nil, err
}
return referenceframe.JointPosToInputs(res), nil
return referenceframe.JointPosToInputs(res)
}

// GoToInputs TODO.
Expand Down
23 changes: 17 additions & 6 deletions component/arm/fake/arm_ik.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,10 +50,17 @@ func NewArmIK(ctx context.Context, cfg config.Component, logger golog.Logger) (a
return nil, err
}

jointPositions := make([]*pb.JointPosition, 6)
for i := 0; i < 6; i++ {
jointPositions[i] = &pb.JointPosition{
JointType: pb.JointPosition_JOINT_TYPE_REVOLUTE,
Parameters: []float64{0.0},
}
}
return &ArmIK{
Name: name,
position: &commonpb.Pose{},
joints: &pb.JointPositions{Degrees: []float64{0, 0, 0, 0, 0, 0}},
joints: jointPositions,
mp: mp,
model: model,
}, nil
Expand All @@ -63,7 +70,7 @@ func NewArmIK(ctx context.Context, cfg config.Component, logger golog.Logger) (a
type ArmIK struct {
Name string
position *commonpb.Pose
joints *pb.JointPositions
joints []*pb.JointPosition
mp motionplan.MotionPlanner
CloseCount int
model referenceframe.Model
Expand All @@ -89,21 +96,25 @@ func (a *ArmIK) MoveToPosition(ctx context.Context, pos *commonpb.Pose, worldSta
if err != nil {
return err
}
solution, err := a.mp.Plan(ctx, pos, referenceframe.JointPosToInputs(joints), nil)
inputs, err := referenceframe.JointPosToInputs(joints)
if err != nil {
return err
}
solution, err := a.mp.Plan(ctx, pos, inputs, nil)
if err != nil {
return err
}
return arm.GoToWaypoints(ctx, a, solution)
}

// MoveToJointPositions sets the joints.
func (a *ArmIK) MoveToJointPositions(ctx context.Context, joints *pb.JointPositions) error {
func (a *ArmIK) MoveToJointPositions(ctx context.Context, joints []*pb.JointPosition) error {
a.joints = joints
return nil
}

// GetJointPositions returns the set joints.
func (a *ArmIK) GetJointPositions(ctx context.Context) (*pb.JointPositions, error) {
func (a *ArmIK) GetJointPositions(ctx context.Context) ([]*pb.JointPosition, error) {
return a.joints, nil
}

Expand All @@ -113,7 +124,7 @@ func (a *ArmIK) CurrentInputs(ctx context.Context) ([]referenceframe.Input, erro
if err != nil {
return nil, err
}
return referenceframe.JointPosToInputs(res), nil
return referenceframe.JointPosToInputs(res)
}

// GoToInputs TODO.
Expand Down
5 changes: 2 additions & 3 deletions component/arm/server.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,8 +68,7 @@ func (s *subtypeServer) GetJointPositions(
if err != nil {
return nil, err
}
convertedPos := &pb.JointPositions{Degrees: pos.Degrees}
return &pb.GetJointPositionsResponse{PositionDegs: convertedPos}, nil
return &pb.GetJointPositionsResponse{JointPositions: pos}, nil
}

// MoveToPosition returns the position of the arm specified.
Expand All @@ -92,5 +91,5 @@ func (s *subtypeServer) MoveToJointPositions(
if err != nil {
return nil, err
}
return &pb.MoveToJointPositionsResponse{}, arm.MoveToJointPositions(ctx, req.PositionDegs)
return &pb.MoveToJointPositionsResponse{}, arm.MoveToJointPositions(ctx, req.JointPositions)
}
Loading