Skip to content
This repository was archived by the owner on Oct 4, 2023. It is now read-only.

Commit dfa2dac

Browse files
twahrburg-googletwahrburg-google
committed
Pyreach sync 20220406
1 parent f84550b commit dfa2dac

File tree

7 files changed

+101
-11
lines changed

7 files changed

+101
-11
lines changed

‎pyreach/examples/arm_fk_quality.py‎

Lines changed: 76 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,76 @@
1+
"""Tests the quality of Forward Kinematics (FK) vs pose provided by the robot.
2+
3+
Works as follows:
4+
1. Connects to robot and loads data.
5+
2. Gets the state of the robot.
6+
3. Compares FK of the joint angles of the robot state with the pose in the state
7+
4. Compares FK again, with the tip adjust computed by PyReach and in FK.
8+
"""
9+
10+
import math
11+
import time
12+
from typing import List
13+
14+
from absl import app # type: ignore
15+
from absl import flags # type: ignore
16+
import numpy as np
17+
from pyreach.common.base import transform_util
18+
from pyreach.factory import ConnectionFactory
19+
20+
21+
def main(unused_argv: List[str]) -> None:
22+
with ConnectionFactory(
23+
connection_string=flags.FLAGS.connection_string,
24+
enable_streaming=False).connect() as host:
25+
while host.config.calibration is None or host.config.constraint is None:
26+
if host.is_closed():
27+
return
28+
time.sleep(0.001)
29+
arm = host.arm
30+
assert arm
31+
print("URDF file:", arm.arm_type.urdf_file)
32+
state = arm.fetch_state()
33+
if not state:
34+
print("State not loaded")
35+
return
36+
goal = arm.fk(state.joint_angles, False)
37+
print("State Flange pose:", state.pose.as_list())
38+
if goal:
39+
print(" FK Flange pose:", goal.as_list())
40+
delta = [(x - y) for x, y in zip(state.pose.as_list(), goal.as_list())]
41+
print(" Diff:", delta)
42+
print(" Pose Delta:", math.sqrt(sum([x * x for x in delta[0:3]])))
43+
print(
44+
" Multiply:",
45+
transform_util.multiply_pose(
46+
np.array(goal.as_list()),
47+
transform_util.inverse_pose(np.array(
48+
state.pose.as_list()))).tolist())
49+
else:
50+
print("FK failed")
51+
tip_adjust_t_base = state.tip_adjust_t_base
52+
if tip_adjust_t_base:
53+
goal = arm.fk(state.joint_angles, True)
54+
print("State Tool pose:", tip_adjust_t_base.as_list())
55+
if goal:
56+
print(" FK Tool pose:", goal.as_list())
57+
delta = [
58+
(x - y) for x, y in zip(tip_adjust_t_base.as_list(), goal.as_list())
59+
]
60+
print(" Diff:", delta)
61+
print(" Pose Delta:", math.sqrt(sum([x * x for x in delta[0:3]])))
62+
print(
63+
" Multiply:",
64+
transform_util.multiply_pose(
65+
np.array(goal.as_list()),
66+
transform_util.inverse_pose(
67+
np.array(tip_adjust_t_base.as_list()))).tolist())
68+
else:
69+
print("FK failed")
70+
71+
72+
if __name__ == "__main__":
73+
flags.DEFINE_string(
74+
"connection_string", "", "Connect using a PyReach connection string (see "
75+
"connection_string.md for examples and documentation).")
76+
app.run(main)

‎pyreach/gyms/devices/arm_device.py‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -194,6 +194,10 @@ def __str__(self) -> str:
194194
return "ReachDeviceArm('{0}':'{1}')".format(self.config_name,
195195
self._reach_name)
196196

197+
def get_apply_tip_adjust_transform(self) -> bool:
198+
"""Return the apply tip adjust transform flag."""
199+
return self._apply_tip_adjust_transform
200+
197201
def _joints_ok(self, joints: Tuple[float, ...]) -> bool:
198202
"""Validate that the joint angles are OK.
199203

‎pyreach/gyms/devices/reach_device.py‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -91,6 +91,10 @@ def allowed_actions(self) -> Set[str]:
9191
"""Return allowed action names."""
9292
return self._allowed_actions
9393

94+
def get_apply_tip_adjust_transform(self) -> bool:
95+
"""Return the apply tip adjust transform for an arm."""
96+
raise pyreach.PyReachError(f"Device {self._config_name} is not an arm")
97+
9498
def _timers_select(self, timer_names: Set[str]) -> internal.TimersSet:
9599
"""Select timers to enable/disable for a block of code.
96100

‎pyreach/gyms/reach_env.py‎

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -703,10 +703,12 @@ def close(self) -> None:
703703
log_contents: str = snap_shot_log_file.read()
704704
json.loads(log_contents)
705705

706-
def fk(self,
707-
element: str,
708-
joints: Union[Tuple[float, ...], List[float], np.ndarray],
709-
apply_tip_adjust_transform: bool = False) -> Optional[pyreach.Pose]:
706+
def fk(
707+
self,
708+
element: str,
709+
joints: Union[Tuple[float, ...], List[float], np.ndarray],
710+
apply_tip_adjust_transform: Optional[bool] = None
711+
) -> Optional[pyreach.Pose]:
710712
"""Uses forward kinematics to get the pose from the joint angles.
711713
712714
Args:
@@ -724,6 +726,8 @@ def fk(self,
724726
arm = self._elements.get(element)
725727
if arm is None or not isinstance(arm, ReachDeviceArm):
726728
return None
729+
if apply_tip_adjust_transform is None:
730+
apply_tip_adjust_transform = arm.get_apply_tip_adjust_transform()
727731
return arm.fk(self._host, joints, apply_tip_adjust_transform)
728732

729733
def set_agent_id(self, agent_id: str) -> None:

‎pyreach/impl/arm_impl.py‎

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -552,6 +552,8 @@ def to_reach_script(
552552
pose_unity[:3].tolist(),
553553
transform_util.axis_angle_to_quaternion(pose_unity[3:]).tolist())
554554
else:
555+
tip_adjust_transform = transform_util.inverse_pose(
556+
tip_adjust_transform)
555557
pose = transform_util.multiply_pose(pose, tip_adjust_transform)
556558
else:
557559
raise core.PyReachError("Calibration was not loaded")
@@ -1403,6 +1405,9 @@ def _update_data_cache(self) -> Optional[_ArmDataCache]:
14031405
if (dev.device_type == "object" and dev.sub_type == "tip" and
14041406
dev.tool_mount in {"robot", "ur"} and
14051407
isinstance(dev, calibration.CalibrationObject)):
1408+
if (tip_dev and tip_dev.device_name.find("tip0") == 0 and
1409+
dev.device_name.find("tip0") != 0):
1410+
continue
14061411
tip_dev = cast(calibration.CalibrationObject, dev)
14071412
if tip_dev.extrinsics:
14081413
tip_transform = np.array(tip_dev.extrinsics, dtype=np.float64)
@@ -1419,9 +1424,6 @@ def _update_data_cache(self) -> Optional[_ArmDataCache]:
14191424
tip_adjust_transform,
14201425
np.array(tip_adjust_dev.extrinsics, dtype=np.float64))
14211426

1422-
tip_transform = transform_util.inverse_pose(tip_transform)
1423-
tip_adjust_transform = transform_util.inverse_pose(tip_adjust_transform)
1424-
14251427
arm_calibration_dev = calib.get_device("robot", self.device_name)
14261428
if arm_calibration_dev is None:
14271429
arm_calibration_dev = calib.get_device("ur", self.device_name)

‎pyreach/impl/arm_impl_test.py‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -775,14 +775,14 @@ def _verify_state(self, state: arm.ArmState) -> None:
775775
self.assertIsNotNone(tip_adjust)
776776
assert tip_adjust
777777
self.assertEqual(tip_adjust.as_tuple(),
778-
(-0.004453485238087255, 0.055702600244064024,
779-
-0.1626512213476071, 0.0, 0.0, 0.0))
778+
(0.004453485238087255, -0.055702600244064024,
779+
0.1626512213476071, 0.0, 0.0, 0.0))
780780
pose_with_tip_adjust = state.tip_adjust_t_base
781781
self.assertIsNotNone(pose_with_tip_adjust)
782782
assert pose_with_tip_adjust
783783
self.assertEqual(
784784
pose_with_tip_adjust.as_tuple(),
785-
(0.20666041510735556, -0.6601364983804328, 0.32705505075589714,
785+
(0.18556403170793967, -0.7691736787189849, 0.0015124760385194225,
786786
0.11865414779070117, -3.075739605978814, -0.0166605496802987))
787787

788788

‎pyreach/impl/reach_tools_impl.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@
1818
},
1919
}
2020

21-
REACH_LINUX_X86_64_HASH = "6bb1ac602e7c28d1818a3783a2f0b72f0ee0445347c9652c83194860c90688a8"
21+
REACH_LINUX_X86_64_HASH = "959e2125e7130a404408ee24a9f4f88b5e0e267e03330e62ac4ad448e62d40e2"
2222
_REACH_SHA256 = {
2323
"linux": {
2424
"x86_64":

0 commit comments

Comments
 (0)