Skip to content

Commit

Permalink
Merge branch 'master' into dock
Browse files Browse the repository at this point in the history
heuristicus authored Oct 18, 2022
2 parents 9534f97 + e71e726 commit 8470d4e
Showing 21 changed files with 782 additions and 9 deletions.
5 changes: 5 additions & 0 deletions README.md
Original file line number Diff line number Diff line change
@@ -31,6 +31,7 @@ default values:
| `SPOT_URDF_EXTRAS` | `empty.urdf` | Optional URDF file to add additional joints and links to the robot |
| `SPOT_JOY_DEVICE` | `/dev/input/js0` | The Linux joypad input device used by the `joy_teleop` node |
| `SPOT_JOY_CONFIG` | `spot_control/config/teleop.yaml` | Joypad button/axis configuration file for `joy_teleop` |
| `SPOT_ARM` | `0` | If `1`, adds the Spot arm to the URDF |


# Building Quick-Start
@@ -99,3 +100,7 @@ catkin_make --cmake-args -DCMAKE_BUILD_TYPE=Release -DPYTHON_EXECUTABLE=/usr/bin
4. re-run `source devel/setup.bash`

5. start the driver with `roslaunch spot_driver driver.launch`

# MoveIt simulation of Spot's arm

Can be found in other repo: https://github.com/estherRay/Spot-Arm
Binary file added spot_description/meshes/arm/base_link.stl
Binary file not shown.
Binary file added spot_description/meshes/arm/finger.stl
Binary file not shown.
Binary file added spot_description/meshes/arm/gripper.stl
Binary file not shown.
Binary file added spot_description/meshes/arm/link1.stl
Binary file not shown.
Binary file added spot_description/meshes/arm/link2.stl
Binary file not shown.
Binary file added spot_description/meshes/arm/link3.stl
Binary file not shown.
Binary file added spot_description/meshes/arm/link4.stl
Binary file not shown.
Binary file added spot_description/meshes/arm/wrist.stl
Binary file not shown.
4 changes: 4 additions & 0 deletions spot_description/urdf/accessories.urdf.xacro
Original file line number Diff line number Diff line change
@@ -77,4 +77,8 @@
</joint>
</xacro:if>
</xacro:if>

<xacro:if value="$(optenv SPOT_ARM 0)">
<xacro:include filename="$(find spot_description)/urdf/spot_arm.xacro" />
</xacro:if>
</robot>
238 changes: 238 additions & 0 deletions spot_description/urdf/spot_arm.xacro
Original file line number Diff line number Diff line change
@@ -0,0 +1,238 @@
<?xml version="1.0" ?>
<robot name="spot_arm" xmlns:xacro="http://www.ros.org/wiki/xacro">

<material name="silver">
<color rgba="0.700 0.700 0.700 1.000"/>
</material>

<link name="base_arm_link">
<inertial>
<origin rpy="0 0 0" xyz="1.2371844231029972e-17 0.0 0.0325"/>
<mass value="5.770791545379091"/>
<inertia ixx="0.007226" ixy="-0.0" ixz="0.0" iyy="0.007226" iyz="0.0" izz="0.010387"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://spot_description/meshes/arm/base_link.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://spot_description/meshes/arm/base_link.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<link name="link4">
<inertial>
<origin rpy="0 0 0" xyz="-0.00911464892336129 0.12606054498393937 5.551115123125783e-17"/>
<mass value="10.95245995004642"/>
<inertia ixx="0.066789" ixy="0.011971" ixz="-0.0" iyy="0.014993" iyz="0.0" izz="0.071949"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.001 -0.4505 -0.0475"/>
<geometry>
<mesh filename="package://spot_description/meshes/arm/link4.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.001 -0.4505 -0.0475"/>
<geometry>
<mesh filename="package://spot_description/meshes/arm/link4.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<link name="gripper">
<inertial>
<origin rpy="0 0 0" xyz="-8.366123908053191e-08 0.07051371901138614 0.017123167388316526"/>
<mass value="5.9780007036604434"/>
<inertia ixx="0.01723" ixy="0.0" ixz="0.0" iyy="0.00734" iyz="-0.003871" izz="0.0153"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.0085 -0.77959 -0.0475"/>
<geometry>
<mesh filename="package://spot_description/meshes/arm/gripper.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.0085 -0.77959 -0.0475"/>
<geometry>
<mesh filename="package://spot_description/meshes/arm/gripper.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<link name="finger">
<inertial>
<origin rpy="0 0 0" xyz="0.8635899028063472 0.09862056591557622 0.00850232975194119"/>
<mass value="1.71236178687828"/>
<inertia ixx="0.002129" ixy="0.0" ixz="-0.0" iyy="0.001681" iyz="0.000162" izz="0.003403"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.87709 -0.823505 -0.0185"/>
<geometry>
<mesh filename="package://spot_description/meshes/arm/finger.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.87709 -0.823505 -0.0185"/>
<geometry>
<mesh filename="package://spot_description/meshes/arm/finger.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<link name="link2">
<inertial>
<origin rpy="0 0 0" xyz="-0.0163095589512068 0.17540914221218962 0.0"/>
<mass value="12.658281999999998"/>
<inertia ixx="0.12979" ixy="0.002625" ixz="0.0" iyy="0.01004" iyz="0.0" izz="0.129493"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="-0.015 -0.0 -0.115"/>
<geometry>
<mesh filename="package://spot_description/meshes/arm/link2.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.015 -0.0 -0.115"/>
<geometry>
<mesh filename="package://spot_description/meshes/arm/link2.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<link name="link1">
<inertial>
<origin rpy="0 0 0" xyz="0.05071972800114364 -1.0308563411658574e-17 0.04676911608650795"/>
<mass value="6.925827706354272"/>
<inertia ixx="0.01241" ixy="-0.0" ixz="-0.001047" iyy="0.010387" iyz="0.0" izz="0.012306"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="-0.0 -0.0 -0.065"/>
<geometry>
<mesh filename="package://spot_description/meshes/arm/link1.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.0 -0.0 -0.065"/>
<geometry>
<mesh filename="package://spot_description/meshes/arm/link1.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<link name="wrist">
<inertial>
<origin rpy="0 0 0" xyz="0.03954999709763633 0.03472006124891969 4.209155544249166e-08"/>
<mass value="3.2278801394086396"/>
<inertia ixx="0.00339" ixy="-2.1e-05" ixz="0.0" iyy="0.002242" iyz="-0.0" izz="0.003439"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.0485 -0.69859 -0.0475"/>
<geometry>
<mesh filename="package://spot_description/meshes/arm/wrist.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.0485 -0.69859 -0.0475"/>
<geometry>
<mesh filename="package://spot_description/meshes/arm/wrist.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<link name="link3">
<inertial>
<origin rpy="0 0 0" xyz="-0.015621881612358529 0.06665620790957599 -0.058074231003688394"/>
<mass value="7.368455487741179"/>
<inertia ixx="0.018073" ixy="0.000915" ixz="-0.00101" iyy="0.011789" iyz="0.003728" izz="0.01667"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="-0.017 -0.3125 -0.115"/>
<geometry>
<mesh filename="package://spot_description/meshes/arm/link3.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.017 -0.3125 -0.115"/>
<geometry>
<mesh filename="package://spot_description/meshes/arm/link3.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<joint name="base_arm_joint" type="fixed">
<origin xyz="0.29 0 0.08" rpy="0 0 0" />
<parent link="body" />
<child link="base_arm_link" />
</joint>

<joint name="arm_joint1" type="revolute">
<origin rpy="0 0 0" xyz="0.0 0.0 0.065"/>
<parent link="base_arm_link"/>
<child link="link1"/>
<axis xyz="0.0 0.0 1.0"/>
<limit effort="100" lower="-3.141593" upper="2.617994" velocity="100"/>
</joint>

<joint name="arm_joint2" type="revolute">
<origin rpy="0 0 0" xyz="0.015 0.0 0.05"/>
<parent link="link1"/>
<child link="link2"/>
<axis xyz="-1.0 0.0 0.0"/>
<limit effort="100" lower="-3.665191" upper="0.0" velocity="100"/>
</joint>

<joint name="arm_joint3" type="revolute">
<origin rpy="0 0 0" xyz="0.002 0.3125 0.0"/>
<parent link="link2"/>
<child link="link3"/>
<axis xyz="1.0 0.0 0.0"/>
<limit effort="100" lower="0.0" upper="2.96706" velocity="100"/>
</joint>

<joint name="arm_joint4" type="revolute">
<origin rpy="0 0 0" xyz="-0.018 0.138 -0.0675"/>
<parent link="link3"/>
<child link="link4"/>
<axis xyz="0.0 1.0 0.0"/>
<limit effort="100" lower="-2.879793" upper="2.879793" velocity="100"/>
</joint>

<joint name="arm_joint5" type="revolute">
<origin rpy="0 0 0" xyz="-0.0475 0.24809 0.0"/>
<parent link="link4"/>
<child link="wrist"/>
<axis xyz="1.0 0.0 0.0"/>
<limit effort="100" lower="-1.570796" upper="1.570796" velocity="100"/>
</joint>

<joint name="arm_joint6" type="revolute">
<origin rpy="0 0 0" xyz="0.04 0.081 0.0"/>
<parent link="wrist"/>
<child link="gripper"/>
<axis xyz="-0.0 1.0 0.0"/>
<limit effort="100" lower="-2.96706" upper="2.96706" velocity="100"/>
</joint>

<joint name="arm_gripper" type="revolute">
<origin rpy="0 0 0" xyz="-0.86859 0.043915 -0.029"/>
<parent link="gripper"/>
<child link="finger"/>
<axis xyz="1.0 -0.0 0.0"/>
<limit effort="100" lower="-1.396263" upper="0.349066" velocity="100"/>
</joint>

</robot>
1 change: 1 addition & 0 deletions spot_driver/config/spot_ros.yaml
Original file line number Diff line number Diff line change
@@ -9,6 +9,7 @@ rates:
front_image: 10.0
side_image: 10.0
rear_image: 10.0
hand_image: 10.0
feedback: 10.0
mobility_params: 10.0
auto_claim: False
14 changes: 14 additions & 0 deletions spot_driver/src/spot_driver/ros_helpers.py
Original file line number Diff line number Diff line change
@@ -41,6 +41,16 @@
friendly_joint_names["hr.hy"] = "rear_right_hip_y"
friendly_joint_names["hr.kn"] = "rear_right_knee"

# arm joints
friendly_joint_names["arm0.sh0"] = "arm_joint1"
friendly_joint_names["arm0.sh1"] = "arm_joint2"
friendly_joint_names["arm0.el0"] = "arm_joint3"
friendly_joint_names["arm0.el1"] = "arm_joint4"
friendly_joint_names["arm0.wr0"] = "arm_joint5"
friendly_joint_names["arm0.wr1"] = "arm_joint6"
friendly_joint_names["arm0.f1x"] = "arm_gripper"


class DefaultCameraInfo(CameraInfo):
"""Blank class extending CameraInfo ROS topic that defaults most parameters"""
def __init__(self):
@@ -202,6 +212,10 @@ def GetJointStatesFromState(state, spot_wrapper):
local_time = spot_wrapper.robotToLocalTime(state.kinematic_state.acquisition_timestamp)
joint_state.header.stamp = rospy.Time(local_time.seconds, local_time.nanos)
for joint in state.kinematic_state.joint_states:
# there is a joint with name arm0.hr0 in the robot state, however this
# joint has no data and should not be there, this is why we ignore it
if joint.name == 'arm0.hr0':
continue
joint_state.name.append(friendly_joint_names.get(joint.name, "ERROR"))
joint_state.position.append(joint.position.value)
joint_state.velocity.append(joint.velocity.value)
Loading

0 comments on commit 8470d4e

Please sign in to comment.