RSDK-8714: Arm wrappers (#66)

core/sdk/src/main/java/com/viam/sdk/core/resource/ResourceManager.java

@@ -2,6 +2,7 @@
 
 import com.viam.common.v1.Common;
 import com.viam.common.v1.Common.ResourceName;
+import com.viam.component.arm.v1.ArmServiceGrpc;
 import com.viam.component.board.v1.BoardServiceGrpc;
 import com.viam.component.camera.v1.CameraServiceGrpc;
 import com.viam.component.gantry.v1.GantryServiceGrpc;
@@ -11,6 +12,7 @@
 import com.viam.component.movementsensor.v1.MovementSensorServiceGrpc;
 import com.viam.component.powersensor.v1.PowerSensorServiceGrpc;
 import com.viam.component.sensor.v1.SensorServiceGrpc;
+import com.viam.sdk.core.component.arm.*;
 import com.viam.component.servo.v1.ServoServiceGrpc;
 import com.viam.sdk.core.component.board.Board;
 import com.viam.sdk.core.component.board.BoardRPCClient;
@@ -113,6 +115,12 @@ public class ResourceManager implements Closeable {
                 PowerSensorRPCService::new,
                 PowerSensorRPCClient::new
         ));
+        Registry.registerSubtype(new ResourceRegistration<>(
+                Arm.SUBTYPE,
+                ArmServiceGrpc.SERVICE_NAME,
+                ArmRPCService::new,
+                ArmRPCClient::new
+        ));
         Registry.registerSubtype(new ResourceRegistration<>(
                 Servo.SUBTYPE,
                 ServoServiceGrpc.SERVICE_NAME,

core/sdk/src/main/kotlin/com/viam/sdk/core/component/arm/Arm.kt

@@ -0,0 +1,158 @@
+package com.viam.sdk.core.component.arm
+
+import com.google.protobuf.ByteString
+import com.google.protobuf.Struct
+import com.viam.common.v1.Common.*
+import com.viam.component.arm.v1.Arm.JointPositions
+import com.viam.sdk.core.component.Component
+import com.viam.sdk.core.component.arm.Arm
+import com.viam.sdk.core.resource.Resource
+import com.viam.sdk.core.resource.Subtype
+import com.viam.sdk.core.robot.RobotClient
+
+/**
+ * An Arm represents a physical robot arm that exists in three-dimensional space.
+ */
+abstract class Arm(name: String) : Component(SUBTYPE, named(name)) {
+    companion object {
+        @JvmField
+        val SUBTYPE = Subtype(Subtype.NAMESPACE_RDK, Subtype.RESOURCE_TYPE_COMPONENT, "arm")
+
+        /**
+         * Get the ResourceName of the component
+         * @param name the name of the component
+         * @return     the component's ResourceName
+         */
+        @JvmStatic
+        fun named(name: String): ResourceName {
+            return Resource.named(SUBTYPE, name)
+        }
+
+        /**
+         * Get the component with the provided name from the provided robot.
+         * @param robot the RobotClient
+         * @param name  the name of the component
+         * @return      the component
+         */
+        @JvmStatic
+        fun fromRobot(robot: RobotClient, name: String): Arm {
+            return robot.getResource(Arm::class.java, named(name))
+        }
+    }
+
+    /**
+     * Get the current position of the end of the arm expressed as a ``Pose``.
+     * @return     A representation of the arm's current position as a 6 DOF (six degrees of freedom) pose.
+     *             The ``Pose`` is composed of values for location and orientation with respect to the origin.
+     *             Location is expressed as distance, which is represented by x, y, and z coordinate values.
+     *             Orientation is expressed as an orientation vector, which is represented by o_x, o_y, o_z, and theta values.
+     */
+    abstract fun getEndPosition(extra: Struct): Pose
+
+    /**
+     * Get the current position of the end of the arm expressed as a ``Pose``.
+     * @return     A representation of the arm's current position as a 6 DOF (six degrees of freedom) pose.
+     *             The ``Pose`` is composed of values for location and orientation with respect to the origin.
+     *             Location is expressed as distance, which is represented by x, y, and z coordinate values.
+     *             Orientation is expressed as an orientation vector, which is represented by o_x, o_y, o_z, and theta values.
+     */
+    fun getEndPosition(): Pose {
+        return getEndPosition(Struct.getDefaultInstance())
+    }
+
+    /**
+     * Move the end of the arm to the Pose specified in ``pose``.
+     * @param pose The destination ``Pose`` for the arm. The ``Pose`` is composed of values for location and orientation
+     *             with respect to the origin.
+     *             Location is expressed as distance, which is represented by x, y, and z coordinate values.
+     *             Orientation is expressed as an orientation vector, which is represented by o_x, o_y, o_z, and theta values.
+
+     */
+    abstract fun moveToPosition(pose: Pose, extra: Struct)
+
+    /**
+     * Move the end of the arm to the Pose specified in ``pose``.
+     * @param pose The destination ``Pose`` for the arm. The ``Pose`` is composed of values for location and orientation
+     *             with respect to the origin.
+     *             Location is expressed as distance, which is represented by x, y, and z coordinate values.
+     *             Orientation is expressed as an orientation vector, which is represented by o_x, o_y, o_z, and theta values.
+
+     */
+    fun moveToPosition(pose: Pose) {
+        moveToPosition(pose, Struct.getDefaultInstance())
+    }
+
+    /**
+     * Move each joint on the arm to the corresponding angle specified in ``positions``.
+     * @param positions the destination ``JointPositions`` for the arm
+     */
+    abstract fun moveToJointPositions(positions: JointPositions, extra: Struct)
+
+    /**
+     * Move each joint on the arm to the corresponding angle specified in ``positions``.
+     * @param positions the destination ``JointPositions`` for the arm
+     */
+    fun moveToJointPositions(positions: JointPositions) {
+        return moveToJointPositions(positions, Struct.getDefaultInstance())
+    }
+
+    /**
+     * Get the JointPositions representing the current position of the arm.
+     * @return The current ``JointPositions`` for the arm.
+     *          ``JointPositions`` can have one attribute, ``values``, a list of joint positions with rotational values (degrees)
+     *           and translational values (mm).
+     */
+    abstract fun getJointPositions(extra: Struct): JointPositions
+
+    /**
+     * Get the JointPositions representing the current position of the arm.
+     * @return The current ``JointPositions`` for the arm.
+     *          ``JointPositions`` can have one attribute, ``values``, a list of joint positions with rotational values (degrees)
+     *           and translational values (mm).
+     */
+    fun getJointPositions(): JointPositions {
+        return getJointPositions(Struct.getDefaultInstance())
+    }
+
+
+    /**
+     * Stop all motion of the arm. It is assumed that the arm stops immediately.
+     */
+    abstract fun stop(extra: Struct)
+
+    /**
+     * Stop all motion of the arm. It is assumed that the arm stops immediately.
+     */
+    fun stop() {
+        stop(Struct.getDefaultInstance())
+    }
+
+    /**
+     * Get if the arm is currently moving.
+     * @return whether the arm is moving
+     */
+    abstract fun isMoving(): Boolean
+
+
+    /**
+     * Get the kinematics information associated with the arm.
+     * @return  A pair containing two values; the first [0] value represents the format of the
+     *          file, either in URDF format (``KinematicsFileFormat.KINEMATICS_FILE_FORMAT_URDF``) or
+     *          Viam's kinematic parameter format (spatial vector algebra) (``KinematicsFileFormat.KINEMATICS_FILE_FORMAT_SVA``),
+     *          and the second [1] value represents the byte contents of the file.
+     */
+    abstract fun getKinematics(extra: Struct): Pair<KinematicsFileFormat, ByteString>
+
+    /**
+     * Get the kinematics information associated with the arm.
+     * @return  A pair containing two values; the first [0] value represents the format of the
+     *          file, either in URDF format (``KinematicsFileFormat.KINEMATICS_FILE_FORMAT_URDF``) or
+     *          Viam's kinematic parameter format (spatial vector algebra) (``KinematicsFileFormat.KINEMATICS_FILE_FORMAT_SVA``),
+     *          and the second [1] value represents the byte contents of the file.
+     */
+    fun getKinematics(): Pair<KinematicsFileFormat, ByteString> {
+        return getKinematics(Struct.getDefaultInstance())
+    }
+
+
+}

core/sdk/src/main/kotlin/com/viam/sdk/core/component/arm/ArmRPCClient.kt

@@ -0,0 +1,83 @@
+package com.viam.sdk.core.component.arm
+
+import com.google.protobuf.ByteString
+import com.google.protobuf.Struct
+import com.google.protobuf.Value
+import com.viam.common.v1.Common
+import com.viam.common.v1.Common.GetGeometriesRequest
+import com.viam.common.v1.Common.GetKinematicsRequest
+import com.viam.component.arm.v1.Arm.*
+import com.viam.component.arm.v1.ArmServiceGrpc
+import com.viam.component.arm.v1.ArmServiceGrpc.ArmServiceBlockingStub
+import com.viam.sdk.core.rpc.Channel
+import java.util.*
+import kotlin.jvm.optionals.getOrDefault
+
+class ArmRPCClient(name: String, channel: Channel) : Arm(name) {
+    private val client: ArmServiceBlockingStub
+
+    init {
+        val client = ArmServiceGrpc.newBlockingStub(channel)
+        if (channel.callCredentials.isPresent) {
+            this.client = client.withCallCredentials(channel.callCredentials.get())
+        } else {
+            this.client = client
+        }
+    }
+
+    override fun getEndPosition(extra: Struct): Common.Pose {
+        val request = GetEndPositionRequest.newBuilder().setName(this.name.name).setExtra(extra).build()
+        val response = this.client.getEndPosition(request)
+        return response.pose
+    }
+
+    override fun moveToPosition(pose: Common.Pose, extra: Struct) {
+        val request = MoveToPositionRequest.newBuilder().setName(this.name.name).setTo(pose).setExtra(extra).build()
+        this.client.moveToPosition(request)
+
+    }
+
+    override fun moveToJointPositions(positions: JointPositions, extra: Struct) {
+        val request =
+            MoveToJointPositionsRequest.newBuilder().setName(this.name.name).setPositions(positions).setExtra(extra)
+                .build()
+        this.client.moveToJointPositions(request)
+    }
+
+    override fun getJointPositions(extra: Struct): JointPositions {
+        val request = GetJointPositionsRequest.newBuilder().setName(this.name.name).setExtra(extra).build()
+        val response = this.client.getJointPositions(request)
+        return response.positions
+    }
+
+    override fun stop(extra: Struct) {
+        val request = StopRequest.newBuilder().setName(this.name.name).setExtra(extra).build()
+        this.client.stop(request)
+    }
+
+    override fun isMoving(): Boolean {
+        val request = IsMovingRequest.newBuilder().setName(this.name.name).build()
+        val response = this.client.isMoving(request)
+        return response.isMoving
+    }
+
+    override fun getKinematics(extra: Struct): Pair<Common.KinematicsFileFormat, ByteString> {
+        val request = GetKinematicsRequest.newBuilder().setName(this.name.name).setExtra(extra).build()
+        val response = this.client.getKinematics(request)
+        return (response.format to response.kinematicsData)
+    }
+
+    override fun doCommand(command: Map<String, Value>?): Struct {
+        val request = Common.DoCommandRequest.newBuilder().setName(this.name.name)
+            .setCommand(Struct.newBuilder().putAllFields(command).build()).build()
+        val response = this.client.doCommand(request)
+        return response.result
+    }
+
+    override fun getGeometries(extra: Optional<Struct>): List<Common.Geometry> {
+        val request = GetGeometriesRequest.newBuilder().setName(this.name.name)
+            .setExtra(extra.getOrDefault(Struct.getDefaultInstance())).build()
+        val response = this.client.getGeometries(request)
+        return response.geometriesList
+    }
+}

core/sdk/src/main/kotlin/com/viam/sdk/core/component/arm/ArmRPCService.kt

@@ -0,0 +1,111 @@
+package com.viam.sdk.core.component.arm
+
+import com.viam.common.v1.Common.*
+import com.viam.component.arm.v1.Arm.*
+import com.viam.component.arm.v1.ArmServiceGrpc
+import com.viam.sdk.core.resource.ResourceManager
+import com.viam.sdk.core.resource.ResourceRPCService
+import io.grpc.stub.StreamObserver
+import java.util.*
+
+internal class ArmRPCService(private val manager: ResourceManager) : ArmServiceGrpc.ArmServiceImplBase(),
+    ResourceRPCService<Arm> {
+    override fun getEndPosition(
+        request: GetEndPositionRequest,
+        responseObserver: StreamObserver<GetEndPositionResponse>
+    ) {
+        val arm = getResource(Arm.named(request.name))
+        val endPosition = arm.getEndPosition(request.extra)
+        responseObserver.onNext(GetEndPositionResponse.newBuilder().setPose(endPosition).build())
+        responseObserver.onCompleted()
+    }
+
+    override fun moveToPosition(
+        request: MoveToPositionRequest,
+        responseObserver: StreamObserver<MoveToPositionResponse>
+    ) {
+        val arm = getResource(Arm.named(request.name))
+        arm.moveToPosition(request.to, request.extra)
+        responseObserver.onNext(MoveToPositionResponse.newBuilder().build())
+        responseObserver.onCompleted()
+    }
+
+    override fun getJointPositions(
+        request: GetJointPositionsRequest,
+        responseObserver: StreamObserver<GetJointPositionsResponse>
+    ) {
+        val arm = getResource(Arm.named(request.name))
+        val positions = arm.getJointPositions(request.extra)
+        responseObserver.onNext(GetJointPositionsResponse.newBuilder().setPositions(positions).build())
+        responseObserver.onCompleted()
+    }
+
+    override fun moveToJointPositions(
+        request: MoveToJointPositionsRequest,
+        responseObserver: StreamObserver<MoveToJointPositionsResponse>
+    ) {
+        val arm = getResource(Arm.named(request.name))
+        arm.moveToJointPositions(request.positions, request.extra)
+        responseObserver.onNext(MoveToJointPositionsResponse.newBuilder().build())
+        responseObserver.onCompleted()
+    }
+
+    override fun getKinematics(
+        request: GetKinematicsRequest,
+        responseObserver: StreamObserver<GetKinematicsResponse>
+    ) {
+        val arm = getResource(Arm.named(request.name))
+        val kinematics = arm.getKinematics(request.extra)
+        responseObserver.onNext(
+            GetKinematicsResponse.newBuilder().setFormat(kinematics.first).setKinematicsData(kinematics.second).build()
+        )
+        responseObserver.onCompleted()
+    }
+
+    override fun stop(
+        request: StopRequest, responseObserver: StreamObserver<StopResponse>
+    ) {
+        val arm = getResource(Arm.named(request.name))
+        arm.stop(request.extra)
+        responseObserver.onNext(StopResponse.newBuilder().build())
+        responseObserver.onCompleted()
+    }
+
+
+    override fun isMoving(
+        request: IsMovingRequest, responseObserver: StreamObserver<IsMovingResponse>
+    ) {
+        val arm = getResource(Arm.named(request.name))
+        val isMoving = arm.isMoving()
+        responseObserver.onNext(
+            IsMovingResponse.newBuilder().setIsMoving(isMoving).build()
+        )
+        responseObserver.onCompleted()
+    }
+
+    override fun doCommand(
+        request: DoCommandRequest, responseObserver: StreamObserver<DoCommandResponse>
+    ) {
+        val arm = getResource(Arm.named(request.name))
+        val result = arm.doCommand(request.command.fieldsMap)
+        responseObserver.onNext(DoCommandResponse.newBuilder().setResult(result).build())
+        responseObserver.onCompleted()
+    }
+
+    override fun getGeometries(
+        request: GetGeometriesRequest, responseObserver: StreamObserver<GetGeometriesResponse>
+    ) {
+        val arm = getResource(Arm.named(request.name))
+        val result = arm.getGeometries(Optional.of(request.extra))
+        responseObserver.onNext(GetGeometriesResponse.newBuilder().addAllGeometries(result).build())
+        responseObserver.onCompleted()
+    }
+
+    override fun getResourceClass(): Class<Arm> {
+        return Arm::class.java
+    }
+
+    override fun getManager(): ResourceManager {
+        return this.manager
+    }
+}