Merge pull request #129 from pollen-robotics/126-add-single-joint-goto

126 add single joint goto
pollen-robotics · Aug 5, 2024 · 66469c5 · 66469c5
2 parents e0139d5 + c41c8c0
commit 66469c5
Show file tree

Hide file tree

Showing 13 changed files with 1,396 additions and 386 deletions.
diff --git a/csharp/Arm.cs b/csharp/Arm.cs
diff --git a/csharp/Goto.cs b/csharp/Goto.cs
diff --git a/csharp/Head.cs b/csharp/Head.cs
diff --git a/protos/arm.proto b/protos/arm.proto
@@ -33,6 +33,16 @@ enum ArmField {
     ALL = 15;
 }
 
+enum ArmJoints {
+    SHOULDER_PITCH = 0;
+    SHOULDER_ROLL = 1;
+    ELBOW_YAW = 2;
+    ELBOW_PITCH = 3;
+    WRIST_ROLL = 4;
+    WRIST_PITCH = 5;
+    WRIST_YAW = 6;
+}
+
 enum IKConstrainedMode {
     UNDEFINED_CONSTRAINED_MODE = 0;
     UNCONSTRAINED = 1;
@@ -55,6 +65,10 @@ enum ReachabilityError {
     OTHER = 6;
 }
 
+message CustomArmJoints {
+    repeated ArmJoints joints = 1;
+}
+
 message ArmState {
     google.protobuf.Timestamp timestamp = 1;
 

diff --git a/protos/goto.proto b/protos/goto.proto
@@ -1,6 +1,7 @@
 syntax = "proto3";
 
 import "google/protobuf/empty.proto";
+import "google/protobuf/wrappers.proto";
 
 import "arm.proto";
 import "head.proto";
@@ -31,18 +32,29 @@ message GoToGoalStatus {
 
 message CartesianGoal {
     oneof cartesian_goal {
-        reachy.part.arm.ArmCartesianGoal arm_cartesian_goal=1;
-        reachy.part.head.NeckCartesianGoal neck_cartesian_goal=2;
+        reachy.part.arm.ArmCartesianGoal arm_cartesian_goal = 1;
+        reachy.part.head.NeckCartesianGoal neck_cartesian_goal = 2;
     }
 }
 
 message JointsGoal {
     oneof joints_goal {
-        reachy.part.arm.ArmJointGoal arm_joint_goal=1;
-        reachy.part.head.NeckJointGoal neck_joint_goal=2;
+        reachy.part.arm.ArmJointGoal arm_joint_goal = 1;
+        reachy.part.head.NeckJointGoal neck_joint_goal = 2;
+        CustomJointGoal custom_joint_goal = 3;
     }
 }
 
+message CustomJointGoal {
+    reachy.part.PartId id = 1;
+    oneof joints {
+        reachy.part.arm.CustomArmJoints arm_joints = 2;
+        reachy.part.head.CustomNeckJoints neck_joints = 3;
+    }
+    repeated google.protobuf.FloatValue joints_goals = 4;
+    google.protobuf.FloatValue duration = 10;
+}
+
 enum InterpolationMode {
     NONE_INTERPOLATION = 0;
     LINEAR = 1;
@@ -55,8 +67,8 @@ message GoToInterpolation {
 
 message GoToRequest {
     oneof goal {
-        CartesianGoal cartesian_goal=1;
-        JointsGoal joints_goal=2;
+        CartesianGoal cartesian_goal = 1;
+        JointsGoal joints_goal = 2;
     }
     GoToInterpolation interpolation_mode = 3;
 }

diff --git a/protos/head.proto b/protos/head.proto
@@ -34,6 +34,16 @@ enum HeadField {
     ALL = 15;
 }
 
+enum NeckJoints {
+    ROLL = 0;
+    PITCH = 1;
+    YAW = 2;
+}
+
+message CustomNeckJoints {
+    repeated NeckJoints joints = 1;
+}
+
 message Head {
     PartId part_id = 1;
     HeadDescription description = 2;

diff --git a/python/reachy2_sdk_api/arm_pb2.py b/python/reachy2_sdk_api/arm_pb2.py
diff --git a/python/reachy2_sdk_api/arm_pb2.pyi b/python/reachy2_sdk_api/arm_pb2.pyi
@@ -64,6 +64,31 @@ PID: ArmField.ValueType  # 12
 ALL: ArmField.ValueType  # 15
 global___ArmField = ArmField
 
+class _ArmJoints:
+    ValueType = typing.NewType("ValueType", builtins.int)
+    V: typing_extensions.TypeAlias = ValueType
+
+class _ArmJointsEnumTypeWrapper(google.protobuf.internal.enum_type_wrapper._EnumTypeWrapper[_ArmJoints.ValueType], builtins.type):
+    DESCRIPTOR: google.protobuf.descriptor.EnumDescriptor
+    SHOULDER_PITCH: _ArmJoints.ValueType  # 0
+    SHOULDER_ROLL: _ArmJoints.ValueType  # 1
+    ELBOW_YAW: _ArmJoints.ValueType  # 2
+    ELBOW_PITCH: _ArmJoints.ValueType  # 3
+    WRIST_ROLL: _ArmJoints.ValueType  # 4
+    WRIST_PITCH: _ArmJoints.ValueType  # 5
+    WRIST_YAW: _ArmJoints.ValueType  # 6
+
+class ArmJoints(_ArmJoints, metaclass=_ArmJointsEnumTypeWrapper): ...
+
+SHOULDER_PITCH: ArmJoints.ValueType  # 0
+SHOULDER_ROLL: ArmJoints.ValueType  # 1
+ELBOW_YAW: ArmJoints.ValueType  # 2
+ELBOW_PITCH: ArmJoints.ValueType  # 3
+WRIST_ROLL: ArmJoints.ValueType  # 4
+WRIST_PITCH: ArmJoints.ValueType  # 5
+WRIST_YAW: ArmJoints.ValueType  # 6
+global___ArmJoints = ArmJoints
+
 class _IKConstrainedMode:
     ValueType = typing.NewType("ValueType", builtins.int)
     V: typing_extensions.TypeAlias = ValueType
@@ -123,6 +148,22 @@ CONTINUITY_LIMIT: ReachabilityError.ValueType  # 5
 OTHER: ReachabilityError.ValueType  # 6
 global___ReachabilityError = ReachabilityError
 
+@typing_extensions.final
+class CustomArmJoints(google.protobuf.message.Message):
+    DESCRIPTOR: google.protobuf.descriptor.Descriptor
+
+    JOINTS_FIELD_NUMBER: builtins.int
+    @property
+    def joints(self) -> google.protobuf.internal.containers.RepeatedScalarFieldContainer[global___ArmJoints.ValueType]: ...
+    def __init__(
+        self,
+        *,
+        joints: collections.abc.Iterable[global___ArmJoints.ValueType] | None = ...,
+    ) -> None: ...
+    def ClearField(self, field_name: typing_extensions.Literal["joints", b"joints"]) -> None: ...
+
+global___CustomArmJoints = CustomArmJoints
+
 @typing_extensions.final
 class ArmState(google.protobuf.message.Message):
     DESCRIPTOR: google.protobuf.descriptor.Descriptor

diff --git a/python/reachy2_sdk_api/goto_pb2.py b/python/reachy2_sdk_api/goto_pb2.py
diff --git a/python/reachy2_sdk_api/goto_pb2.pyi b/python/reachy2_sdk_api/goto_pb2.pyi
@@ -9,7 +9,9 @@ import google.protobuf.descriptor
 import google.protobuf.internal.containers
 import google.protobuf.internal.enum_type_wrapper
 import google.protobuf.message
+import google.protobuf.wrappers_pb2
 import head_pb2
+import part_pb2
 import sys
 import typing
 
@@ -137,22 +139,60 @@ class JointsGoal(google.protobuf.message.Message):
 
     ARM_JOINT_GOAL_FIELD_NUMBER: builtins.int
     NECK_JOINT_GOAL_FIELD_NUMBER: builtins.int
+    CUSTOM_JOINT_GOAL_FIELD_NUMBER: builtins.int
     @property
     def arm_joint_goal(self) -> arm_pb2.ArmJointGoal: ...
     @property
     def neck_joint_goal(self) -> head_pb2.NeckJointGoal: ...
+    @property
+    def custom_joint_goal(self) -> global___CustomJointGoal: ...
     def __init__(
         self,
         *,
         arm_joint_goal: arm_pb2.ArmJointGoal | None = ...,
         neck_joint_goal: head_pb2.NeckJointGoal | None = ...,
+        custom_joint_goal: global___CustomJointGoal | None = ...,
     ) -> None: ...
-    def HasField(self, field_name: typing_extensions.Literal["arm_joint_goal", b"arm_joint_goal", "joints_goal", b"joints_goal", "neck_joint_goal", b"neck_joint_goal"]) -> builtins.bool: ...
-    def ClearField(self, field_name: typing_extensions.Literal["arm_joint_goal", b"arm_joint_goal", "joints_goal", b"joints_goal", "neck_joint_goal", b"neck_joint_goal"]) -> None: ...
-    def WhichOneof(self, oneof_group: typing_extensions.Literal["joints_goal", b"joints_goal"]) -> typing_extensions.Literal["arm_joint_goal", "neck_joint_goal"] | None: ...
+    def HasField(self, field_name: typing_extensions.Literal["arm_joint_goal", b"arm_joint_goal", "custom_joint_goal", b"custom_joint_goal", "joints_goal", b"joints_goal", "neck_joint_goal", b"neck_joint_goal"]) -> builtins.bool: ...
+    def ClearField(self, field_name: typing_extensions.Literal["arm_joint_goal", b"arm_joint_goal", "custom_joint_goal", b"custom_joint_goal", "joints_goal", b"joints_goal", "neck_joint_goal", b"neck_joint_goal"]) -> None: ...
+    def WhichOneof(self, oneof_group: typing_extensions.Literal["joints_goal", b"joints_goal"]) -> typing_extensions.Literal["arm_joint_goal", "neck_joint_goal", "custom_joint_goal"] | None: ...
 
 global___JointsGoal = JointsGoal
 
+@typing_extensions.final
+class CustomJointGoal(google.protobuf.message.Message):
+    DESCRIPTOR: google.protobuf.descriptor.Descriptor
+
+    ID_FIELD_NUMBER: builtins.int
+    ARM_JOINTS_FIELD_NUMBER: builtins.int
+    NECK_JOINTS_FIELD_NUMBER: builtins.int
+    JOINTS_GOALS_FIELD_NUMBER: builtins.int
+    DURATION_FIELD_NUMBER: builtins.int
+    @property
+    def id(self) -> part_pb2.PartId: ...
+    @property
+    def arm_joints(self) -> arm_pb2.CustomArmJoints: ...
+    @property
+    def neck_joints(self) -> head_pb2.CustomNeckJoints: ...
+    @property
+    def joints_goals(self) -> google.protobuf.internal.containers.RepeatedCompositeFieldContainer[google.protobuf.wrappers_pb2.FloatValue]: ...
+    @property
+    def duration(self) -> google.protobuf.wrappers_pb2.FloatValue: ...
+    def __init__(
+        self,
+        *,
+        id: part_pb2.PartId | None = ...,
+        arm_joints: arm_pb2.CustomArmJoints | None = ...,
+        neck_joints: head_pb2.CustomNeckJoints | None = ...,
+        joints_goals: collections.abc.Iterable[google.protobuf.wrappers_pb2.FloatValue] | None = ...,
+        duration: google.protobuf.wrappers_pb2.FloatValue | None = ...,
+    ) -> None: ...
+    def HasField(self, field_name: typing_extensions.Literal["arm_joints", b"arm_joints", "duration", b"duration", "id", b"id", "joints", b"joints", "neck_joints", b"neck_joints"]) -> builtins.bool: ...
+    def ClearField(self, field_name: typing_extensions.Literal["arm_joints", b"arm_joints", "duration", b"duration", "id", b"id", "joints", b"joints", "joints_goals", b"joints_goals", "neck_joints", b"neck_joints"]) -> None: ...
+    def WhichOneof(self, oneof_group: typing_extensions.Literal["joints", b"joints"]) -> typing_extensions.Literal["arm_joints", "neck_joints"] | None: ...
+
+global___CustomJointGoal = CustomJointGoal
+
 @typing_extensions.final
 class GoToInterpolation(google.protobuf.message.Message):
     DESCRIPTOR: google.protobuf.descriptor.Descriptor