| @@ -54,7 +54,7 @@ jobs: | |||
| run: | | |||
| sudo apt update | |||
| sudo apt-get install portaudio19-dev | |||
| sudo apt-get install libdav1d-dev nasm | |||
| sudo apt-get install libdav1d-dev nasm libudev-dev | |||
| mkdir -p $HOME/.rustup/toolchains/stable-x86_64-unknown-linux-gnu/lib/rustlib/x86_64-unknown-linux-gnu/lib | |||
| ln -s /lib/x86_64-linux-gnu/libdav1d.so $HOME/.rustup/toolchains/stable-x86_64-unknown-linux-gnu/lib/rustlib/x86_64-unknown-linux-gnu/lib/libdav1d.so | |||
| @@ -6505,26 +6505,6 @@ dependencies = [ | |||
| "redox_syscall 0.5.10", | |||
| ] | |||
| [[package]] | |||
| name = "libudev" | |||
| version = "0.3.0" | |||
| source = "registry+https://github.com/rust-lang/crates.io-index" | |||
| checksum = "78b324152da65df7bb95acfcaab55e3097ceaab02fb19b228a9eb74d55f135e0" | |||
| dependencies = [ | |||
| "libc", | |||
| "libudev-sys", | |||
| ] | |||
| [[package]] | |||
| name = "libudev-sys" | |||
| version = "0.1.4" | |||
| source = "registry+https://github.com/rust-lang/crates.io-index" | |||
| checksum = "3c8469b4a23b962c1396b9b451dda50ef5b283e8dd309d69033475fa9b334324" | |||
| dependencies = [ | |||
| "libc", | |||
| "pkg-config", | |||
| ] | |||
| [[package]] | |||
| name = "libz-sys" | |||
| version = "1.1.22" | |||
| @@ -12456,7 +12436,6 @@ dependencies = [ | |||
| "core-foundation 0.10.0", | |||
| "core-foundation-sys", | |||
| "io-kit-sys", | |||
| "libudev", | |||
| "mach2", | |||
| "nix 0.26.4", | |||
| "scopeguard", | |||
| @@ -7,7 +7,7 @@ nodes: | |||
| outputs: | |||
| - pose | |||
| env: | |||
| PORT: /dev/ttyACM1 | |||
| PORT: /dev/ttyACM0 | |||
| IDS: 1 2 3 4 5 6 | |||
| TORQUE: false | |||
| @@ -31,13 +31,12 @@ nodes: | |||
| env: | |||
| URDF_PATH: so100.urdf | |||
| END_EFFECTOR_LINK: "Moving Jaw" | |||
| so100_transform: -0.18 0.02 -0.65 0.7 0 0 0.7 | |||
| TRANSFORM: -0.18 0.02 -0.65 0.7 0 0 0.7 | |||
| - id: plot | |||
| build: pip install -e ../../node-hub/dora-rerun | |||
| path: dora-rerun | |||
| inputs: | |||
| series_so100: so100/pose | |||
| series_fk: pytorch-kinematics/pose | |||
| jointstate_so100: so100/pose | |||
| camera/image: camera/image | |||
| @@ -7,18 +7,26 @@ from dora import Node | |||
| node = Node() | |||
| target_x = -0.08 | |||
| target_y = -0.20 | |||
| target_y = -0.02 | |||
| target_x = 0.00 | |||
| place_x = -0.08 | |||
| place_y = 0.20 | |||
| place_x = -0.02 | |||
| place_y = -0.1 | |||
| top_z = -0.50 | |||
| low_z = -0.57 | |||
| roll = 1.86 | |||
| pitch = 1.43 | |||
| yaw_closed = 0.8 | |||
| yaw_opened = -0.5 | |||
| now = time.time() | |||
| time.sleep(1.5) | |||
| node.send_output( | |||
| "action", | |||
| pa.array([target_y, target_x, 0.15, -1.6, -0.0, -1]), | |||
| pa.array([target_x, target_y, top_z, roll, pitch, yaw_closed]), | |||
| metadata={"encoding": "xyzrpy"}, | |||
| ) | |||
| @@ -26,7 +34,7 @@ time.sleep(0.8) | |||
| node.send_output( | |||
| "action", | |||
| pa.array([target_y, target_x, 0.15, -1.6, -0.0, -1]), | |||
| pa.array([target_x, target_y, top_z, roll, pitch, yaw_closed]), | |||
| metadata={"encoding": "xyzrpy"}, | |||
| ) | |||
| @@ -34,7 +42,7 @@ time.sleep(0.5) | |||
| node.send_output( | |||
| "action", | |||
| pa.array([target_y, target_x, 0.09, -1.6, -0.0, -1]), | |||
| pa.array([target_x, target_y, low_z, roll, pitch, yaw_closed]), | |||
| metadata={"encoding": "xyzrpy"}, | |||
| ) | |||
| time.sleep(0.2) | |||
| @@ -42,7 +50,7 @@ time.sleep(0.2) | |||
| node.send_output( | |||
| "action", | |||
| pa.array([target_y, target_x, 0.09, -1.6, -0.0, -3]), | |||
| pa.array([target_x, target_y, low_z, roll, pitch, yaw_opened]), | |||
| metadata={"encoding": "xyzrpy"}, | |||
| ) | |||
| @@ -51,7 +59,7 @@ time.sleep(1.0) | |||
| node.send_output( | |||
| "action", | |||
| pa.array([target_y, target_x, 0.15, -1.6, -0.0, -3]), | |||
| pa.array([target_x, target_y, top_z, roll, pitch, yaw_opened]), | |||
| metadata={"encoding": "xyzrpy"}, | |||
| ) | |||
| @@ -60,7 +68,7 @@ time.sleep(0.3) | |||
| node.send_output( | |||
| "action", | |||
| pa.array([place_y, place_x, 0.15, -1.6, -0.0, -3]), | |||
| pa.array([place_x, place_y, top_z, roll, pitch, yaw_opened]), | |||
| metadata={"encoding": "xyzrpy"}, | |||
| ) | |||
| @@ -68,7 +76,7 @@ time.sleep(1.0) | |||
| node.send_output( | |||
| "action", | |||
| pa.array([place_y, place_x, 0.10, -1.6, -0.0, -3]), | |||
| pa.array([place_x, place_y, low_z, roll, pitch, yaw_opened]), | |||
| metadata={"encoding": "xyzrpy"}, | |||
| ) | |||
| @@ -76,14 +84,14 @@ time.sleep(0.2) | |||
| node.send_output( | |||
| "action", | |||
| pa.array([place_y, place_x, 0.10, -1.6, -0.0, -1]), | |||
| pa.array([place_x, place_y, low_z, roll, pitch, yaw_closed]), | |||
| metadata={"encoding": "xyzrpy"}, | |||
| ) | |||
| time.sleep(1.0) | |||
| node.send_output( | |||
| "action", | |||
| pa.array([place_y, place_x, 0.15, -1.6, -0.0, -3]), | |||
| pa.array([place_x, place_y, top_z, roll, pitch, yaw_opened]), | |||
| metadata={"encoding": "xyzrpy"}, | |||
| ) | |||
| @@ -91,6 +99,6 @@ time.sleep(1.0) | |||
| node.send_output( | |||
| "action", | |||
| pa.array([place_y, place_x, 0.15, -1.6, -0.0, -3]), | |||
| pa.array([place_x, place_y, top_z, roll, pitch, yaw_opened]), | |||
| metadata={"encoding": "xyzrpy"}, | |||
| ) | |||
| @@ -95,13 +95,9 @@ | |||
| <child | |||
| link="Rotation_Pitch" /> | |||
| <axis | |||
| xyz="0 1 0" /> | |||
| xyz="0 -1 0" /> | |||
| <!-- note for the so100 arm there is no well defined effort/velocity limits at the moment --> | |||
| <limit | |||
| lower="-2.1" | |||
| upper="2.1" | |||
| effort="0" | |||
| velocity="0" /> | |||
| </joint> | |||
| <link | |||
| name="Upper_Arm"> | |||
| @@ -148,18 +144,14 @@ | |||
| type="revolute"> | |||
| <origin | |||
| xyz="0 0.1025 0.0306" | |||
| rpy="1.5708 0 0" /> | |||
| rpy="0 0 0" /> | |||
| <parent | |||
| link="Rotation_Pitch" /> | |||
| <child | |||
| link="Upper_Arm" /> | |||
| <axis | |||
| xyz="-1 0 0" /> | |||
| <limit | |||
| lower="-0.1" | |||
| upper="3.45" | |||
| effort="0" | |||
| velocity="0" /> | |||
| xyz="1 0 0" /> | |||
| </joint> | |||
| <link | |||
| name="Lower_Arm"> | |||
| @@ -206,18 +198,13 @@ | |||
| type="revolute"> | |||
| <origin | |||
| xyz="0 0.11257 0.028" | |||
| rpy="-1.5708 0 0" /> | |||
| rpy="-0.1 0 0" /> | |||
| <parent | |||
| link="Upper_Arm" /> | |||
| <child | |||
| link="Lower_Arm" /> | |||
| <axis | |||
| xyz="1 0 0" /> | |||
| <limit | |||
| lower="-0.2" | |||
| upper="3.14159" | |||
| effort="0" | |||
| velocity="0" /> | |||
| </joint> | |||
| <link | |||
| name="Wrist_Pitch_Roll"> | |||
| @@ -322,7 +309,7 @@ | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Wrist_Roll_Follower_SO101.stl" /> | |||
| filename="Wrist_Roll_Follower_SO101.STL" /> | |||
| </geometry> | |||
| </collision> | |||
| </link> | |||
| @@ -337,12 +324,8 @@ | |||
| <child | |||
| link="Fixed_Jaw" /> | |||
| <axis | |||
| xyz="0 -1 0" /> | |||
| <limit | |||
| lower="-3.14159" | |||
| upper="3.14159" | |||
| effort="0" | |||
| velocity="0" /> | |||
| xyz="0 1 0" /> | |||
| </joint> | |||
| <link | |||
| name="Moving Jaw"> | |||
| @@ -380,7 +363,7 @@ | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Moving_Jaw_SO101.stl" /> | |||
| filename="Moving_Jaw_SO101.STL" /> | |||
| </geometry> | |||
| </collision> | |||
| </link> | |||
| @@ -396,10 +379,6 @@ | |||
| link="Moving Jaw" /> | |||
| <axis | |||
| xyz="0 0 1" /> | |||
| <limit | |||
| lower="0" | |||
| upper="1.7" | |||
| effort="0" | |||
| velocity="0" /> | |||
| </joint> | |||
| </robot> | |||
| @@ -1,365 +0,0 @@ | |||
| <?xml version="1.0" encoding="utf-8"?> | |||
| <!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) | |||
| Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634 | |||
| For more information, please see http://wiki.ros.org/sw_urdf_exporter --> | |||
| <robot | |||
| name="SO_5DOF_ARM100_8j_URDF.SLDASM"> | |||
| <link | |||
| name="base_link"> | |||
| <inertial> | |||
| <origin | |||
| xyz="-2.45960666746703E-07 0.0311418169687909 0.0175746661003382" | |||
| rpy="0 0 0" /> | |||
| <mass | |||
| value="0.193184127927598" /> | |||
| <inertia | |||
| ixx="0.000137030709467877" | |||
| ixy="2.10136126944992E-08" | |||
| ixz="4.24087422551286E-09" | |||
| iyy="0.000169089551209259" | |||
| iyz="2.26514711036514E-05" | |||
| izz="0.000145097720857224" /> | |||
| </inertial> | |||
| <visual> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Base.STL" /> | |||
| </geometry> | |||
| <material | |||
| name=""> | |||
| <color | |||
| rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" /> | |||
| </material> | |||
| </visual> | |||
| <collision> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Base.STL" /> | |||
| </geometry> | |||
| </collision> | |||
| </link> | |||
| <link | |||
| name="Rotation_Pitch"> | |||
| <inertial> | |||
| <origin | |||
| xyz="-9.07886224712597E-05 0.0590971820568318 0.031089016892169" | |||
| rpy="0 0 0" /> | |||
| <mass | |||
| value="0.119226314127197" /> | |||
| <inertia | |||
| ixx="5.90408775624429E-05" | |||
| ixy="4.90800532852998E-07" | |||
| ixz="-5.90451772654387E-08" | |||
| iyy="3.21498601038881E-05" | |||
| iyz="-4.58026206663885E-06" | |||
| izz="5.86058514263952E-05" /> | |||
| </inertial> | |||
| <visual> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Rotation_Pitch.STL" /> | |||
| </geometry> | |||
| <material | |||
| name=""> | |||
| <color | |||
| rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" /> | |||
| </material> | |||
| </visual> | |||
| <collision> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Rotation_Pitch.STL" /> | |||
| </geometry> | |||
| </collision> | |||
| </link> | |||
| <joint | |||
| name="Rotation" | |||
| type="continuous"> | |||
| <origin | |||
| xyz="0 -0.0452 0.0165" | |||
| rpy="1.5708 0 0" /> | |||
| <parent | |||
| link="base_link" /> | |||
| <child | |||
| link="Rotation_Pitch" /> | |||
| <axis | |||
| xyz="0 -1 0" /> | |||
| </joint> | |||
| <link | |||
| name="Upper_Arm"> | |||
| <inertial> | |||
| <origin | |||
| xyz="-1.7205170190925E-05 0.0701802156327694 0.00310545118155671" | |||
| rpy="0 0 0" /> | |||
| <mass | |||
| value="0.162409284599177" /> | |||
| <inertia | |||
| ixx="0.000167153146617081" | |||
| ixy="1.03902689187701E-06" | |||
| ixz="-1.20161820645189E-08" | |||
| iyy="7.01946992214245E-05" | |||
| iyz="2.11884806298698E-06" | |||
| izz="0.000213280241160769" /> | |||
| </inertial> | |||
| <visual> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Upper_Arm.STL" /> | |||
| </geometry> | |||
| <material | |||
| name=""> | |||
| <color | |||
| rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" /> | |||
| </material> | |||
| </visual> | |||
| <collision> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Upper_Arm.STL" /> | |||
| </geometry> | |||
| </collision> | |||
| </link> | |||
| <joint | |||
| name="Pitch" | |||
| type="continuous"> | |||
| <origin | |||
| xyz="0 0.1025 0.0306" | |||
| rpy="0 0 0" /> | |||
| <parent | |||
| link="Rotation_Pitch" /> | |||
| <child | |||
| link="Upper_Arm" /> | |||
| <axis | |||
| xyz="1 0 0" /> | |||
| </joint> | |||
| <link | |||
| name="Lower_Arm"> | |||
| <inertial> | |||
| <origin | |||
| xyz="-0.00339603710186651 0.00137796353960074 0.0768006751156044" | |||
| rpy="0 0 0" /> | |||
| <mass | |||
| value="0.147967774582291" /> | |||
| <inertia | |||
| ixx="0.000105333995841409" | |||
| ixy="1.73059237226499E-07" | |||
| ixz="-1.1720305455211E-05" | |||
| iyy="0.000138766654485212" | |||
| iyz="1.77429964684103E-06" | |||
| izz="5.08741652515214E-05" /> | |||
| </inertial> | |||
| <visual> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Lower_Arm.STL" /> | |||
| </geometry> | |||
| <material | |||
| name=""> | |||
| <color | |||
| rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" /> | |||
| </material> | |||
| </visual> | |||
| <collision> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Lower_Arm.STL" /> | |||
| </geometry> | |||
| </collision> | |||
| </link> | |||
| <joint | |||
| name="Elbow" | |||
| type="continuous"> | |||
| <origin | |||
| xyz="0 0.11257 0.028" | |||
| rpy="0 0 0" /> | |||
| <parent | |||
| link="Upper_Arm" /> | |||
| <child | |||
| link="Lower_Arm" /> | |||
| <axis | |||
| xyz="1 0 0" /> | |||
| </joint> | |||
| <link | |||
| name="Wrist_Pitch_Roll"> | |||
| <inertial> | |||
| <origin | |||
| xyz="-0.00852653127372418 -0.0352278997897927 -2.34622481569413E-05" | |||
| rpy="0 0 0" /> | |||
| <mass | |||
| value="0.066132067097723" /> | |||
| <inertia | |||
| ixx="1.95717492443445E-05" | |||
| ixy="-6.62714374412293E-07" | |||
| ixz="5.20089016442066E-09" | |||
| iyy="2.38028417569933E-05" | |||
| iyz="4.09549055863776E-08" | |||
| izz="3.4540143384536E-05" /> | |||
| </inertial> | |||
| <visual> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Wrist_Pitch_Roll.STL" /> | |||
| </geometry> | |||
| <material | |||
| name=""> | |||
| <color | |||
| rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" /> | |||
| </material> | |||
| </visual> | |||
| <collision> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Wrist_Pitch_Roll.STL" /> | |||
| </geometry> | |||
| </collision> | |||
| </link> | |||
| <joint | |||
| name="Wrist_Pitch" | |||
| type="continuous"> | |||
| <origin | |||
| xyz="0 0.0052 0.1349" | |||
| rpy="-1.57 0 0" /> | |||
| <parent | |||
| link="Lower_Arm" /> | |||
| <child | |||
| link="Wrist_Pitch_Roll" /> | |||
| <axis | |||
| xyz="1 0 0" /> | |||
| </joint> | |||
| <link | |||
| name="Fixed_Jaw"> | |||
| <inertial> | |||
| <origin | |||
| xyz="0.00552376906426563 -0.0280167153359021 0.000483582592841092" | |||
| rpy="0 0 0" /> | |||
| <mass | |||
| value="0.0929859131176897" /> | |||
| <inertia | |||
| ixx="4.3328249304211E-05" | |||
| ixy="7.09654328670947E-06" | |||
| ixz="5.99838530879484E-07" | |||
| iyy="3.04451747368212E-05" | |||
| iyz="-1.58743247545413E-07" | |||
| izz="5.02460913506734E-05" /> | |||
| </inertial> | |||
| <visual> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Wrist_Roll_Follower_SO101.STL" /> | |||
| </geometry> | |||
| <material | |||
| name=""> | |||
| <color | |||
| rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" /> | |||
| </material> | |||
| </visual> | |||
| <collision> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Wrist_Roll_Follower_SO101.STL" /> | |||
| </geometry> | |||
| </collision> | |||
| </link> | |||
| <joint | |||
| name="Wrist_Roll" | |||
| type="continuous"> | |||
| <origin | |||
| xyz="0 -0.0601 0" | |||
| rpy="0 1.57 0" /> | |||
| <parent | |||
| link="Wrist_Pitch_Roll" /> | |||
| <child | |||
| link="Fixed_Jaw" /> | |||
| <axis | |||
| xyz="0 1 0" /> | |||
| </joint> | |||
| <link | |||
| name="Moving Jaw"> | |||
| <inertial> | |||
| <origin | |||
| xyz="-0.00161744605468241 -0.0303472584046471 0.000449645961853651" | |||
| rpy="0 0 0" /> | |||
| <mass | |||
| value="0.0202443794940372" /> | |||
| <inertia | |||
| ixx="1.10911325081525E-05" | |||
| ixy="-5.35076503033314E-07" | |||
| ixz="-9.46105662101403E-09" | |||
| iyy="3.03576451001973E-06" | |||
| iyz="-1.71146075110632E-07" | |||
| izz="8.9916083370498E-06" /> | |||
| </inertial> | |||
| <visual> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Moving_Jaw_SO101.STL" /> | |||
| </geometry> | |||
| <material | |||
| name=""> | |||
| <color | |||
| rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" /> | |||
| </material> | |||
| </visual> | |||
| <collision> | |||
| <origin | |||
| xyz="0 0 0" | |||
| rpy="0 0 0" /> | |||
| <geometry> | |||
| <mesh | |||
| filename="Moving_Jaw_SO101.STL" /> | |||
| </geometry> | |||
| </collision> | |||
| </link> | |||
| <joint | |||
| name="Jaw" | |||
| type="continuous"> | |||
| <origin | |||
| xyz="-0.0202 -0.0244 0" | |||
| rpy="0 0 0" /> | |||
| <parent | |||
| link="Fixed_Jaw" /> | |||
| <child | |||
| link="Moving Jaw" /> | |||
| <axis | |||
| xyz="0 0 1" /> | |||
| </joint> | |||
| </robot> | |||
| @@ -8,7 +8,6 @@ nodes: | |||
| - pose | |||
| env: | |||
| PORT: /dev/ttyACM0 | |||
| CONFIG: follower.right.json | |||
| TORQUE: true | |||
| IDS: 1 2 3 4 5 6 | |||
| @@ -17,6 +16,15 @@ nodes: | |||
| outputs: | |||
| - action | |||
| - id: camera | |||
| build: pip install -e ../../node-hub/dora-pyrealsense | |||
| path: dora-pyrealsense | |||
| inputs: | |||
| tick: dora/timer/millis/33 | |||
| outputs: | |||
| - image | |||
| - depth | |||
| - id: pytorch-kinematics | |||
| build: pip install -e ../../node-hub/dora-pytorch-kinematics | |||
| path: dora-pytorch-kinematics | |||
| @@ -27,18 +35,21 @@ nodes: | |||
| - pose | |||
| - action | |||
| env: | |||
| URDF_PATH: so100_2.urdf | |||
| URDF_PATH: so100.urdf | |||
| END_EFFECTOR_LINK: "Moving Jaw" | |||
| so100_transform: 0 -0.3 0 | |||
| TRANSFORM: -0.18 0.02 -0.65 0.7 0 0 0.7 | |||
| - id: plot | |||
| build: pip install -e ../../node-hub/dora-rerun | |||
| path: dora-rerun | |||
| inputs: | |||
| series_so100: so100/pose | |||
| series_pose: pytorch-kinematics/pose | |||
| series_action: pytorch-kinematics/action | |||
| jointstate_so100: so100/pose | |||
| #series_so100: so100/pose | |||
| # series_pose: pytorch-kinematics/pose | |||
| series_so100: pytorch-kinematics/action | |||
| jointstate_so100: pytorch-kinematics/action | |||
| camera/image: camera/image | |||
| camera/depth: camera/depth | |||
| env: | |||
| so100_urdf: so100_2.urdf | |||
| so100_transform: 0 -0.3 0 | |||
| so100_urdf: so100.urdf | |||
| so100_transform: -0.18 0.02 -0.65 0.7 0 0 0.7 | |||
| CAMERA_PITCH: -3.1415 | |||
| @@ -10,6 +10,20 @@ import torch | |||
| from dora import Node | |||
| from pytorch_kinematics.transforms.rotation_conversions import matrix_to_euler_angles | |||
| TRANSFORM = np.array(os.getenv("TRANSFORM", "0. 0. 0. 0. 0. 0. 1.").split(" ")).astype( | |||
| np.float32 | |||
| ) | |||
| pos = torch.tensor([TRANSFORM[0], TRANSFORM[1], TRANSFORM[2]]) | |||
| rot = torch.tensor( | |||
| [ | |||
| TRANSFORM[3], | |||
| TRANSFORM[4], | |||
| TRANSFORM[5], | |||
| TRANSFORM[6], | |||
| ], | |||
| ) | |||
| ROB_TF = pk.Transform3d(pos=pos, rot=rot) | |||
| def get_xyz_rpy_array_from_transform3d( | |||
| transform: "pytorch_kinematics.transforms.Transform3d", convention: str = "XYZ" | |||
| @@ -134,9 +148,6 @@ class RobotKinematics: | |||
| """ | |||
| # robot frame | |||
| pos = torch.tensor([0.0, 0.0, 0.0]) | |||
| rot = torch.tensor([0.0, 0.0, 0.0]) | |||
| rob_tf = pk.Transform3d(pos=pos, rot=rot) | |||
| angles_tensor = self._prepare_joint_tensor( | |||
| joint_angles, batch_dim_required=False | |||
| @@ -145,7 +156,7 @@ class RobotKinematics: | |||
| goal_in_rob_frame_tf = self.chain.forward_kinematics( | |||
| angles_tensor, end_only=True | |||
| ) | |||
| goal_tf = rob_tf.compose(goal_in_rob_frame_tf) | |||
| goal_tf = ROB_TF.compose(goal_in_rob_frame_tf) | |||
| return goal_tf | |||
| def compute_ik( | |||
| @@ -250,6 +261,7 @@ def main(): | |||
| torch.tensor(target[3:6]), convention="XYZ" | |||
| ), | |||
| ) | |||
| target = ROB_TF.inverse().compose(target) | |||
| solution = robot.compute_ik(target, last_known_state) | |||
| if solution is None: | |||
| continue | |||
| @@ -275,7 +287,7 @@ def main(): | |||
| # Apply Forward Kinematics | |||
| target = robot.compute_fk(target) | |||
| target = np.array(get_xyz_rpy_array_from_transform3d(target)) | |||
| target = pa.array(target.ravel()) | |||
| target = pa.array(target.ravel(), type=pa.float32()) | |||
| metadata["encoding"] = "xyzrpy" | |||
| node.send_output(event["id"], target, metadata=metadata) | |||
| @@ -1,4 +1,4 @@ | |||
| use std::collections::HashMap; | |||
| use std::{collections::HashMap, path::PathBuf}; | |||
| use eyre::{Context, ContextCompat, Result}; | |||
| use k::{nalgebra::Quaternion, Chain, Translation3, UnitQuaternion}; | |||
| @@ -64,6 +64,12 @@ pub fn init_urdf(rec: &RecordingStream) -> Result<HashMap<String, Chain<f32>>> { | |||
| let chain = k::Chain::<f32>::from_urdf_file(&urdf_path).context("Could not load URDF")?; | |||
| let transform = key.replace("_urdf", "_transform"); | |||
| if PathBuf::from(&urdf_path).file_name() != PathBuf::from(&path).file_name() { | |||
| return Err(eyre::eyre!( | |||
| "URDF path should be the same as the key without _urdf or _URDF. Got {} instead of {}", urdf_path, path | |||
| )); | |||
| } | |||
| if let Err(err) = rec.log_file_from_path(&urdf_path, None, false) { | |||
| println!("Could not log file: {}. Errored with {}", urdf_path, err); | |||
| println!("Make sure to install urdf loader with:"); | |||
| @@ -71,6 +77,7 @@ pub fn init_urdf(rec: &RecordingStream) -> Result<HashMap<String, Chain<f32>>> { | |||
| "pip install git+https://github.com/rerun-io/rerun-loader-python-example-urdf.git" | |||
| ) | |||
| }; | |||
| // Get transform by replacing URDF_ with TRANSFORM_ | |||
| if let Ok(transform) = std::env::var(transform) { | |||
| let transform = transform | |||
| @@ -9,4 +9,4 @@ edition = "2021" | |||
| dora-node-api = "0.3.11" | |||
| eyre = "0.6.12" | |||
| rustypot = { git = "https://github.com/pollen-robotics/rustypot", branch = "next-release-1.0" } | |||
| serialport = "4.7.1" | |||
| serialport = { version = "4.7.1", default-features = false } | |||