Merge pull request #700 from threedworld-mit/offhand_follows_relative

Offhand follows relative

Documentation/Changelog.md

@@ -4,6 +4,21 @@
 
 To upgrade from TDW v1.11 to v1.12, read [this guide](upgrade_guides/v1.11_to_v1.12.md).
 
+## v1.12.26
+
+### Command API
+
+#### Modified Commands
+
+| Command | Description |
+| --- | --- |
+| `replicant_reach_for_position`<br>`replicant_reach_for_relative_position`<br>`wheelchair_replicant_reach_for_position` | Added parameter `offhand_follows`. |
+
+### `tdw` module
+
+- Fixed: `replicant.reach_for(target, offhand_follows=True, absolute=False)` doesn't move the offhand to the correct position. The code responsbile for calculating the offhand target has been moved from `tdw` into the build.
+- Likewise for `wheelchair_replicant.reach_for(target, offhand_follows=True, absolute=False)`
+
 ## v1.12.25
 
 ### Command API

Documentation/api/command_api.md

@@ -6550,7 +6550,7 @@ Tell the Replicant to start to reach for a target object. The Replicant will try
 ```
 
 ```python
-{"$type": "replicant_reach_for_object", "object_id": 1, "duration": 0.125, "arm": "left", "id": 1, "max_distance": 1.5, "arrived_at": 0.02, "set_status": True, "offset": {"x": 0, "y": 0, "z": 0}}
+{"$type": "replicant_reach_for_object", "object_id": 1, "duration": 0.125, "arm": "left", "id": 1, "max_distance": 1.5, "arrived_at": 0.02, "set_status": True, "offset": {"x": 0, "y": 0, "z": 0}, "offhand_follows": False}
 ```
 
 | Parameter | Type | Description | Default |
@@ -6560,6 +6560,7 @@ Tell the Replicant to start to reach for a target object. The Replicant will try
 | `"arrived_at"` | float | If the hand is this distance from the target position or less, the action succeeded. | 0.02 |
 | `"set_status"` | bool | If True, when this command ends, it will set the Replicant output data's status. | True |
 | `"offset"` | Vector3 | This offset will be applied to the target position. | {"x": 0, "y": 0, "z": 0} |
+| `"offhand_follows"` | bool | If True, the offhand will follow the primary hand, meaning that it will maintain the same relative position. | False |
 | `"duration"` | float | The duration of the motion in seconds. | |
 | `"arm"` | Arm | The arm doing the action. | |
 | `"id"` | int | The unique object ID. | |
@@ -6587,7 +6588,7 @@ Tell a Replicant to start to reach for a target position.
 ```
 
 ```python
-{"$type": "replicant_reach_for_position", "position": {"x": 1.1, "y": 0.0, "z": 0}, "duration": 0.125, "arm": "left", "id": 1, "max_distance": 1.5, "arrived_at": 0.02, "set_status": True, "offset": {"x": 0, "y": 0, "z": 0}}
+{"$type": "replicant_reach_for_position", "position": {"x": 1.1, "y": 0.0, "z": 0}, "duration": 0.125, "arm": "left", "id": 1, "max_distance": 1.5, "arrived_at": 0.02, "set_status": True, "offset": {"x": 0, "y": 0, "z": 0}, "offhand_follows": False}
 ```
 
 | Parameter | Type | Description | Default |
@@ -6597,6 +6598,7 @@ Tell a Replicant to start to reach for a target position.
 | `"arrived_at"` | float | If the hand is this distance from the target position or less, the action succeeded. | 0.02 |
 | `"set_status"` | bool | If True, when this command ends, it will set the Replicant output data's status. | True |
 | `"offset"` | Vector3 | This offset will be applied to the target position. | {"x": 0, "y": 0, "z": 0} |
+| `"offhand_follows"` | bool | If True, the offhand will follow the primary hand, meaning that it will maintain the same relative position. | False |
 | `"duration"` | float | The duration of the motion in seconds. | |
 | `"arm"` | Arm | The arm doing the action. | |
 | `"id"` | int | The unique object ID. | |
@@ -6624,7 +6626,7 @@ Instruct a Replicant to start to reach for a target position relative to the Rep
 ```
 
 ```python
-{"$type": "replicant_reach_for_relative_position", "position": {"x": 1.1, "y": 0.0, "z": 0}, "duration": 0.125, "arm": "left", "id": 1, "max_distance": 1.5, "arrived_at": 0.02, "set_status": True, "offset": {"x": 0, "y": 0, "z": 0}}
+{"$type": "replicant_reach_for_relative_position", "position": {"x": 1.1, "y": 0.0, "z": 0}, "duration": 0.125, "arm": "left", "id": 1, "max_distance": 1.5, "arrived_at": 0.02, "set_status": True, "offset": {"x": 0, "y": 0, "z": 0}, "offhand_follows": False}
 ```
 
 | Parameter | Type | Description | Default |
@@ -6634,6 +6636,7 @@ Instruct a Replicant to start to reach for a target position relative to the Rep
 | `"arrived_at"` | float | If the hand is this distance from the target position or less, the action succeeded. | 0.02 |
 | `"set_status"` | bool | If True, when this command ends, it will set the Replicant output data's status. | True |
 | `"offset"` | Vector3 | This offset will be applied to the target position. | {"x": 0, "y": 0, "z": 0} |
+| `"offhand_follows"` | bool | If True, the offhand will follow the primary hand, meaning that it will maintain the same relative position. | False |
 | `"duration"` | float | The duration of the motion in seconds. | |
 | `"arm"` | Arm | The arm doing the action. | |
 | `"id"` | int | The unique object ID. | |
@@ -6669,7 +6672,7 @@ Tell a WheelchairReplicant to start to reach for a target object. The Wheelchair
 ```
 
 ```python
-{"$type": "wheelchair_replicant_reach_for_object", "object_id": 1, "duration": 0.125, "arm": "left", "id": 1, "max_distance": 1.5, "arrived_at": 0.02, "set_status": True, "offset": {"x": 0, "y": 0, "z": 0}}
+{"$type": "wheelchair_replicant_reach_for_object", "object_id": 1, "duration": 0.125, "arm": "left", "id": 1, "max_distance": 1.5, "arrived_at": 0.02, "set_status": True, "offset": {"x": 0, "y": 0, "z": 0}, "offhand_follows": False}
 ```
 
 | Parameter | Type | Description | Default |
@@ -6680,6 +6683,7 @@ Tell a WheelchairReplicant to start to reach for a target object. The Wheelchair
 | `"set_status"` | bool | If True, when this command ends, it will set the Replicant output data's status. | True |
 | `"offset"` | Vector3 | This offset will be applied to the target position. | {"x": 0, "y": 0, "z": 0} |
 | `"duration"` | float | The duration of the motion in seconds. | |
+| `"offhand_follows"` | bool | If True, the offhand will follow the primary hand, meaning that it will maintain the same relative position. | False |
 | `"arm"` | Arm | The arm doing the action. | |
 | `"id"` | int | The unique object ID. | |
 
@@ -6706,7 +6710,7 @@ Tell a WheelchairReplicant to start to reach for a target position.
 ```
 
 ```python
-{"$type": "wheelchair_replicant_reach_for_position", "position": {"x": 1.1, "y": 0.0, "z": 0}, "absolute": True, "duration": 0.125, "arm": "left", "id": 1, "max_distance": 1.5, "arrived_at": 0.02, "set_status": True, "offset": {"x": 0, "y": 0, "z": 0}}
+{"$type": "wheelchair_replicant_reach_for_position", "position": {"x": 1.1, "y": 0.0, "z": 0}, "absolute": True, "duration": 0.125, "arm": "left", "id": 1, "max_distance": 1.5, "arrived_at": 0.02, "set_status": True, "offset": {"x": 0, "y": 0, "z": 0}, "offhand_follows": False}
 ```
 
 | Parameter | Type | Description | Default |
@@ -6718,6 +6722,7 @@ Tell a WheelchairReplicant to start to reach for a target position.
 | `"set_status"` | bool | If True, when this command ends, it will set the Replicant output data's status. | True |
 | `"offset"` | Vector3 | This offset will be applied to the target position. | {"x": 0, "y": 0, "z": 0} |
 | `"duration"` | float | The duration of the motion in seconds. | |
+| `"offhand_follows"` | bool | If True, the offhand will follow the primary hand, meaning that it will maintain the same relative position. | False |
 | `"arm"` | Arm | The arm doing the action. | |
 | `"id"` | int | The unique object ID. | |
 
@@ -6744,7 +6749,7 @@ Tell a WheelchairReplicant to start to reset the arm to its neutral position.
 ```
 
 ```python
-{"$type": "wheelchair_replicant_reset_arm", "duration": 0.125, "arm": "left", "id": 1, "max_distance": 1.5, "arrived_at": 0.02, "set_status": True, "offset": {"x": 0, "y": 0, "z": 0}}
+{"$type": "wheelchair_replicant_reset_arm", "duration": 0.125, "arm": "left", "id": 1, "max_distance": 1.5, "arrived_at": 0.02, "set_status": True, "offset": {"x": 0, "y": 0, "z": 0}, "offhand_follows": False}
 ```
 
 | Parameter | Type | Description | Default |
@@ -6754,6 +6759,7 @@ Tell a WheelchairReplicant to start to reset the arm to its neutral position.
 | `"set_status"` | bool | If True, when this command ends, it will set the Replicant output data's status. | True |
 | `"offset"` | Vector3 | This offset will be applied to the target position. | {"x": 0, "y": 0, "z": 0} |
 | `"duration"` | float | The duration of the motion in seconds. | |
+| `"offhand_follows"` | bool | If True, the offhand will follow the primary hand, meaning that it will maintain the same relative position. | False |
 | `"arm"` | Arm | The arm doing the action. | |
 | `"id"` | int | The unique object ID. | |
 

Python/tdw/replicant/actions/reach_for.py

@@ -116,32 +116,15 @@ def get_initialization_commands(self, resp: List[bytes], static: ReplicantStatic
         return commands
 
     def _get_reach_for_position(self, target: Dict[str, float], arm: Arm, resp: List[bytes], static: ReplicantStatic, dynamic: ReplicantDynamic) -> List[dict]:
-        commands = [{"$type": "replicant_reach_for_position" if self.absolute else "replicant_reach_for_relative_position",
-                     "id": static.replicant_id,
-                     "position": target,
-                     "duration": self.duration,
-                     "arm": arm.name,
-                     "max_distance": self.max_distance,
-                     "arrived_at": self.arrived_at,
-                     "offset": self._get_offset(arm=arm, resp=resp, static=static, dynamic=dynamic)}]
-        # Tell the offhand to follow.
-        if self.offhand_follows:
-            # Get the offset to the target.
-            offset = TDWUtils.vector3_to_array(target) - dynamic.body_parts[static.hands[self.arms[0]]].position
-            # Get the offhand.
-            offhand = Arm.right if self.arms[0] == Arm.left else Arm.left
-            # Get the position.
-            position = dynamic.body_parts[static.hands[offhand]].position + offset
-            # Set the target of the offhand.
-            commands.append({"$type": "replicant_reach_for_position",
-                             "id": static.replicant_id,
-                             "position": TDWUtils.array_to_vector3(position),
-                             "duration": self.duration,
-                             "arm": offhand.name,
-                             "max_distance": self.max_distance,
-                             "arrived_at": self.arrived_at,
-                             "offset": self._get_offset(arm=offhand, resp=resp, static=static, dynamic=dynamic)})
-        return commands
+        return [{"$type": "replicant_reach_for_position" if self.absolute else "replicant_reach_for_relative_position",
+                 "id": static.replicant_id,
+                 "position": target,
+                 "duration": self.duration,
+                 "arm": arm.name,
+                 "max_distance": self.max_distance,
+                 "arrived_at": self.arrived_at,
+                 "offset": self._get_offset(arm=arm, resp=resp, static=static, dynamic=dynamic),
+                 "offhand_follows": self.offhand_follows}]
 
     def _get_offset(self, arm: Arm, resp: List[bytes], static: ReplicantStatic, dynamic: ReplicantDynamic) -> Dict[str, float]:
         if self.from_held and arm in dynamic.held_objects:

Python/tdw/version.py

@@ -1 +1 @@
-__version__ = "1.12.25.0"
+__version__ = "1.12.26.0"

Python/tdw/wheelchair_replicant/actions/reach_for.py

@@ -129,34 +129,16 @@ def get_end_commands(self, resp: List[bytes], static: ReplicantStatic, dynamic:
 
     def _get_reach_for_position(self, target: Dict[str, float], arm: Arm, resp: List[bytes],
                                 static: ReplicantStatic, dynamic: ReplicantDynamic) -> List[dict]:
-        commands = [{"$type": "wheelchair_replicant_reach_for_position",
-                     "id": static.replicant_id,
-                     "position": target,
-                     "duration": self.duration,
-                     "arm": arm.name,
-                     "max_distance": self.max_distance,
-                     "arrived_at": self.arrived_at,
-                     "absolute": self.absolute,
-                     "offset": self._get_offset(arm=arm, resp=resp, static=static, dynamic=dynamic)}]
-        # Tell the offhand to follow.
-        if self.offhand_follows:
-            # Get the offset to the target.
-            offset = TDWUtils.vector3_to_array(target) - dynamic.body_parts[static.hands[self.arms[0]]].position
-            # Get the offhand.
-            offhand = Arm.right if self.arms[0] == Arm.left else Arm.left
-            # Get the position.
-            position = dynamic.body_parts[static.hands[offhand]].position + offset
-            # Set the target of the offhand.
-            commands.append({"$type": "wheelchair_replicant_reach_for_position",
-                             "id": static.replicant_id,
-                             "position": TDWUtils.array_to_vector3(position),
-                             "duration": self.duration,
-                             "arm": offhand.name,
-                             "max_distance": self.max_distance,
-                             "arrived_at": self.arrived_at,
-                             "absolute": self.absolute,
-                             "offset": self._get_offset(arm=offhand, resp=resp, static=static, dynamic=dynamic)})
-        return commands
+        return [{"$type": "wheelchair_replicant_reach_for_position",
+                 "id": static.replicant_id,
+                 "position": target,
+                 "duration": self.duration,
+                 "arm": arm.name,
+                 "max_distance": self.max_distance,
+                 "arrived_at": self.arrived_at,
+                 "absolute": self.absolute,
+                 "offset": self._get_offset(arm=arm, resp=resp, static=static, dynamic=dynamic),
+                 "offhand_follows": self.offhand_follows}]
 
     def _get_offset(self, arm: Arm, resp: List[bytes], static: ReplicantStatic, dynamic: ReplicantDynamic) -> Dict[str, float]:
         if self.from_held and arm in dynamic.held_objects: