Skip to content

Commit

Permalink
Merge branch 'main' into dev/angels_changes
Browse files Browse the repository at this point in the history 
* main:
  [SW-1106] using custom launch actions from synchros2 (bdaiinstitute#537)
  [SW-1764] allow use of preferred_odom_frame parameter (bdaiinstitute#538)
  [dependabot] Bump ros_utilities from `5aa699a` to `539c962` (bdaiinstitute#535)
  [SW-1658] Allow body frame to be root of TF tree (bdaiinstitute#536)
  [N/a] fix for launching the driver without the gripper (bdaiinstitute#534)
  [MAPLE-681] Add robot_description_package as parameter to driver launch (bdaiinstitute#531)
  [dependabot] Bump ros_utilities from `e4d5c80` to `5aa699a` (bdaiinstitute#530)
  [dependabot] Bump ros_utilities from `02ca14e` to `e4d5c80` (bdaiinstitute#525)
  [dependabot] Bump ros_utilities from `5258a5a` to `02ca14e` (bdaiinstitute#524)
  [dependabot] Bump spot_wrapper from `76e522b` to `c78ebfb` (bdaiinstitute#523)
  [dependabot] Bump ros_utilities from `23a79c9` to `5258a5a` (bdaiinstitute#522)
  [N/A] bugfix: give new xacro arguments default values (bdaiinstitute#521)
  [SW-1507] gripperless URDF for Spot (bdaiinstitute#520)
  [SW-1539] don't publish images from hand camera if gripperless param set (bdaiinstitute#518)
  [SW-1466] ability to take joint gains in through a parameter file (bdaiinstitute#516)

# Conflicts:
#	spot_driver/spot_driver/spot_ros2.py
  • Loading branch information
@AngelRodriguez8008
AngelRodriguez8008 committed Dec 27, 2024
2 parents 507bc66 + 5c17fcd commit 50916a5
Show file tree
Hide file tree
Showing 33 changed files with 538 additions and 292 deletions.
2 changes: 1 addition & 1 deletion ros_utilities
Submodule ros_utilities updated 172 files
8 changes: 8 additions & 0 deletions spot_description/launch/description.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,12 @@ def generate_launch_description() -> launch.LaunchDescription:
choices=["True", "true", "False", "false"],
description="Flag to enable putting frames at the feet",
),
DeclareLaunchArgument(
name="gripperless",
default_value="False",
choices=["True", "true", "False", "false"],
description="Flag to remove the gripper from the arm model",
),
DeclareLaunchArgument(
"tf_prefix", default_value='""', description="Apply namespace prefix to robot links and joints"
),
Expand All @@ -56,6 +62,8 @@ def generate_launch_description() -> launch.LaunchDescription:
LaunchConfiguration("arm"),
" feet:=",
LaunchConfiguration("feet"),
" gripperless:=",
LaunchConfiguration("gripperless"),
" tf_prefix:=",
LaunchConfiguration("tf_prefix"),
]
Expand Down
8 changes: 3 additions & 5 deletions spot_description/urdf/spot.ros2_control.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,10 +20,6 @@
<param name="min">${-effort_max}</param>
<param name="max">${effort_max}</param>
</command_interface>
<!-- TODO at some point, this should be a part of the command interface so we can have a
one-to-one mapping to the spot sdk implementation. -->
<!-- <command_interface name="k_q_p" />
<command_interface name="k_qd_p" /> -->
</joint>
</xacro:macro>

Expand Down Expand Up @@ -65,7 +61,7 @@

</xacro:macro>

<xacro:macro name="spot_ros2_control" params="interface_type has_arm hostname username password tf_prefix">
<xacro:macro name="spot_ros2_control" params="interface_type has_arm hostname username password tf_prefix k_q_p k_qd_p">
<!-- Currently implements a simple system interface that covers all joints of the robot.
In the future, we could make different hardware interfaces for the body, arm, etc. -->
<ros2_control name="SpotSystem" type="system">
Expand All @@ -80,6 +76,8 @@
<param name="hostname">$(optenv SPOT_IP ${hostname})</param>
<param name="username">$(optenv BOSDYN_CLIENT_USERNAME ${username})</param>
<param name="password">$(optenv BOSDYN_CLIENT_PASSWORD ${password})</param>
<param name="k_q_p">${k_q_p}</param>
<param name="k_qd_p">${k_qd_p}</param>
</xacro:if>
</hardware>
<!-- Add the legs -->
Expand Down
21 changes: 17 additions & 4 deletions spot_description/urdf/spot.urdf.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,23 +7,34 @@
<xacro:include filename="$(find spot_description)/urdf/spot.ros2_control.xacro" />

<!-- General parameters -->

<!-- String that will be prepended to all joints and links-->
<xacro:arg name="tf_prefix" default="" />

<!-- Set to true if Spot has an arm -->
<xacro:arg name="arm" default="false" />

<!-- Set to true if Spot has an arm but not a gripper -->
<xacro:arg name="gripperless" default="false" />

<!-- Set to true to put links at the feet of Spot -->
<xacro:arg name="feet" default="false" />
<xacro:arg name="tf_prefix" default="" />

<!-- Parameters for ROS 2 control -->
<xacro:arg name="add_ros2_control_tag" default="false" />
<xacro:arg name="hardware_interface_type" default="mock" />
<xacro:arg name="tf_prefix" default="" />
<xacro:arg name="hostname" default="10.0.0.3" />
<xacro:arg name="username" default="username" />
<xacro:arg name="password" default="password" />
<xacro:arg name="k_q_p" default="" />
<xacro:arg name="k_qd_p" default="" />

<!-- Load Spot -->
<xacro:load_spot
arm="$(arg arm)"
feet="$(arg feet)"
tf_prefix="$(arg tf_prefix)" />
tf_prefix="$(arg tf_prefix)"
gripperless="$(arg gripperless)"/>

<!-- Adding the ROS 2 control tags -->
<xacro:if value="$(arg add_ros2_control_tag)">
Expand All @@ -32,7 +43,9 @@
hostname="$(arg hostname)"
username="$(arg username)"
password="$(arg password)"
tf_prefix="$(arg tf_prefix)" />
tf_prefix="$(arg tf_prefix)"
k_q_p="$(arg k_q_p)"
k_qd_p="$(arg k_qd_p)" />
</xacro:if>

</robot>
215 changes: 120 additions & 95 deletions spot_description/urdf/spot_arm_macro.urdf
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,12 @@
<?xml version="1.0"?>
<robot name="spot_arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="load_arm" params="
tf_prefix">
tf_prefix
gripperless:=false
custom_gripper_base_link:=''">

<!-- Useful boolean variable for determining if custom gripper base link should be used in the xacro -->
<xacro:property name="use_custom_gripper_base_link" value="${gripperless and custom_gripper_base_link!=''}"/>

<link name="${tf_prefix}arm_link_sh0">
<visual>
Expand Down Expand Up @@ -201,107 +206,127 @@
upper="1.83259571459404613236" />
</joint>

<link name="${tf_prefix}arm_link_wr1">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://spot_description/meshes/arm/visual/arm_link_wr1.obj" />
</geometry>
<material name="arm_link_wr1">
<color rgba="0.2 0.2 0.2 1" />
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://spot_description/meshes/arm/collision/arm_link_wr1.obj" />
</geometry>
</collision>
<collision>
<geometry>
<mesh filename="package://spot_description/meshes/arm/collision/front_jaw.obj" />
</geometry>
</collision>
<collision>
<geometry>
<mesh filename="package://spot_description/meshes/arm/collision/middle_jaw.obj" />
</geometry>
</collision>
<collision>
<geometry>
<mesh filename="package://spot_description/meshes/arm/collision/jaw_tooth.obj" />
</geometry>
</collision>
<inertial>
<origin rpy="0.006393076449033863 0.2928526604761865 -0.022814984050994802"
xyz="0.12516802549362183 0.00010137435310753062 -0.013997982256114483" />
<mass value="0.785" />
<inertia ixx="0.00076741362048185" ixy="0.0" ixz="0.0" iyy="0.0017628124054271809" iyz="0.0"
izz="0.0016818105865345951" />
</inertial>
</link>
<xacro:if value="${gripperless}">
<xacro:unless value="${use_custom_gripper_base_link}">
<!-- Here this link needs to still be defined as to not break WR1 joint but can't include the meshes below, as
these include parts of the gripper. Otherwise custom gripper base link will be used as the child of WR1 -->
<link name="${tf_prefix}arm_link_wr1" />
</xacro:unless>
</xacro:if>
<xacro:unless value="${gripperless}">
<link name="${tf_prefix}arm_link_wr1">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://spot_description/meshes/arm/visual/arm_link_wr1.obj" />
</geometry>
<material name="arm_link_wr1">
<color rgba="0.2 0.2 0.2 1" />
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://spot_description/meshes/arm/collision/arm_link_wr1.obj" />
</geometry>
</collision>
<collision>
<geometry>
<mesh filename="package://spot_description/meshes/arm/collision/front_jaw.obj" />
</geometry>
</collision>
<collision>
<geometry>
<mesh filename="package://spot_description/meshes/arm/collision/middle_jaw.obj" />
</geometry>
</collision>
<collision>
<geometry>
<mesh filename="package://spot_description/meshes/arm/collision/jaw_tooth.obj" />
</geometry>
</collision>
<inertial>
<origin rpy="0.006393076449033863 0.2928526604761865 -0.022814984050994802"
xyz="0.12516802549362183 0.00010137435310753062 -0.013997982256114483" />
<mass value="0.785" />
<inertia ixx="0.00076741362048185" ixy="0.0" ixz="0.0" iyy="0.0017628124054271809" iyz="0.0"
izz="0.0016818105865345951" />
</inertial>
</link>
</xacro:unless>
<joint name="${tf_prefix}arm_wr1" type="revolute">
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
<axis xyz="1 0 0" />
<parent link="${tf_prefix}arm_link_wr0" />
<child link="${tf_prefix}arm_link_wr1" />
<!-- Child link can be different depending on gripperless status: -->
<xacro:if value="${use_custom_gripper_base_link}">
<!-- can optionally be the custom link specified via arg -->
<child link="${custom_gripper_base_link}" />
</xacro:if>
<xacro:unless value="${use_custom_gripper_base_link}">
<!-- else defaults to normal arm_link_wr1 -->
<child link="${tf_prefix}arm_link_wr1" />
</xacro:unless>
<limit effort="30.3" velocity="10.0" lower="-2.87979326579064354163"
upper="2.87979326579064354163" />
</joint>

<link name="${tf_prefix}arm_link_fngr">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://spot_description/meshes/arm/visual/arm_link_fngr.obj" />
</geometry>
<material name="arm_link_fngr">
<color rgba="0.2 0.2 0.2 1" />
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://spot_description/meshes/arm/collision/left_hinge.obj" />
</geometry>
</collision>
<collision>
<geometry>
<mesh filename="package://spot_description/meshes/arm/collision/left_finger.obj" />
</geometry>
</collision>
<collision>
<geometry>
<mesh filename="package://spot_description/meshes/arm/collision/left_tooth.obj" />
</geometry>
</collision>
<collision>
<geometry>
<mesh filename="package://spot_description/meshes/arm/collision/right_hinge.obj" />
</geometry>
</collision>
<collision>
<geometry>
<mesh filename="package://spot_description/meshes/arm/collision/right_finger.obj" />
</geometry>
</collision>
<collision>
<geometry>
<mesh filename="package://spot_description/meshes/arm/collision/right_tooth.obj" />
</geometry>
</collision>
<inertial>
<origin rpy="0.0003416659657240473 0.3751670012784243 -0.004058661249495721"
xyz="0.04787873849272728 -0.00020676758140325546 -0.01628788933157921" />
<mass value="0.2" />
<inertia ixx="0.0001439963634171048" ixy="0.0" ixz="0.0" iyy="0.0003007126997998294"
iyz="0.0" izz="0.0003912783268020657" />
</inertial>
</link>
<joint name="${tf_prefix}arm_f1x" type="revolute">
<origin xyz="0.11745 0 0.014820" rpy="0 0 0" />
<axis xyz="0.0 1.0 0.0" />
<parent link="${tf_prefix}arm_link_wr1" />
<child link="${tf_prefix}arm_link_fngr" />
<limit effort="15.32" velocity="10.0" lower="-1.57" upper="0.0" />
</joint>
<!-- Finger joint and link shouldn't be included if gripperless -->
<xacro:unless value="${gripperless}">
<link name="${tf_prefix}arm_link_fngr">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://spot_description/meshes/arm/visual/arm_link_fngr.obj" />
</geometry>
<material name="arm_link_fngr">
<color rgba="0.2 0.2 0.2 1" />
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://spot_description/meshes/arm/collision/left_hinge.obj" />
</geometry>
</collision>
<collision>
<geometry>
<mesh filename="package://spot_description/meshes/arm/collision/left_finger.obj" />
</geometry>
</collision>
<collision>
<geometry>
<mesh filename="package://spot_description/meshes/arm/collision/left_tooth.obj" />
</geometry>
</collision>
<collision>
<geometry>
<mesh filename="package://spot_description/meshes/arm/collision/right_hinge.obj" />
</geometry>
</collision>
<collision>
<geometry>
<mesh filename="package://spot_description/meshes/arm/collision/right_finger.obj" />
</geometry>
</collision>
<collision>
<geometry>
<mesh filename="package://spot_description/meshes/arm/collision/right_tooth.obj" />
</geometry>
</collision>
<inertial>
<origin rpy="0.0003416659657240473 0.3751670012784243 -0.004058661249495721"
xyz="0.04787873849272728 -0.00020676758140325546 -0.01628788933157921" />
<mass value="0.2" />
<inertia ixx="0.0001439963634171048" ixy="0.0" ixz="0.0" iyy="0.0003007126997998294"
iyz="0.0" izz="0.0003912783268020657" />
</inertial>
</link>
<joint name="${tf_prefix}arm_f1x" type="revolute">
<origin xyz="0.11745 0 0.014820" rpy="0 0 0" />
<axis xyz="0.0 1.0 0.0" />
<parent link="${tf_prefix}arm_link_wr1" />
<child link="${tf_prefix}arm_link_fngr" />
<limit effort="15.32" velocity="10.0" lower="-1.57" upper="0.0" />
</joint>
</xacro:unless>
</xacro:macro>
</robot>
6 changes: 5 additions & 1 deletion spot_description/urdf/spot_macro.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,8 @@
<xacro:macro name="load_spot" params="
arm:=false
feet:=false
gripperless:=false
custom_gripper_base_link:=''
tf_prefix">

<link name="${tf_prefix}body">
Expand Down Expand Up @@ -417,7 +419,9 @@
<!-- Include Arm if necessary-->
<xacro:if value="${arm}">
<xacro:include filename="$(find spot_description)/urdf/spot_arm_macro.urdf" />
<xacro:load_arm tf_prefix="${tf_prefix}" />
<xacro:load_arm tf_prefix="${tf_prefix}"
gripperless="${gripperless}"
custom_gripper_base_link="${custom_gripper_base_link}" />
</xacro:if>

<!-- Include links at feet if necessary -->
Expand Down
3 changes: 2 additions & 1 deletion spot_driver/config/spot_ros_example.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,8 @@
estop_timeout: 9.0
start_estop: False

preferred_odom_frame: "odom" # pass either odom/vision. This frame will become the parent of body in tf2 tree and will be used in odometry topic. https://dev.bostondynamics.com/docs/concepts/geometry_and_frames.html?highlight=frame#frames-in-the-spot-robot-world for more info.
preferred_odom_frame: "odom" # pass either odom/vision. This will be used in the odometry topic. https://dev.bostondynamics.com/docs/concepts/geometry_and_frames.html?highlight=frame#frames-in-the-spot-robot-world for more info.
tf_root: "odom" # Use "odom" (default), "vision", or "body" for the root frame in your TF tree.

cmd_duration: 0.25 # The duration of cmd_vel commands. Increase this if spot stutters when publishing cmd_vel.
rgb_cameras: True # Set to False if your robot has greyscale cameras -- otherwise you won't receive data.
Expand Down
7 changes: 4 additions & 3 deletions spot_driver/include/spot_driver/conversions/robot_state.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -134,8 +134,9 @@ std::optional<tf2_msgs::msg::TFMessage> getTf(const ::bosdyn::api::FrameTreeSnap
* @return If the robot state message contains the velocity of the Spot's body relative to the odometry frame in its
* kinematic state, return a TwistWithCovarianceStamped containing this data. Otherwise, return nullopt.
*/
std::optional<geometry_msgs::msg::TwistWithCovarianceStamped> getOdomTwist(
const ::bosdyn::api::RobotState& robot_state, const google::protobuf::Duration& clock_skew);
std::optional<geometry_msgs::msg::TwistWithCovarianceStamped> getOdomTwist(const ::bosdyn::api::RobotState& robot_state,
const google::protobuf::Duration& clock_skew,
const bool is_using_vision);

/**
* @brief Create an Odometry ROS message representing Spot's pose and velocity relative to a fixed world frame by
Expand All @@ -152,7 +153,7 @@ std::optional<geometry_msgs::msg::TwistWithCovarianceStamped> getOdomTwist(
*/
std::optional<nav_msgs::msg::Odometry> getOdom(const ::bosdyn::api::RobotState& robot_state,
const google::protobuf::Duration& clock_skew, const std::string& prefix,
bool is_using_vision);
const bool is_using_vision);

/**
* @brief Create a PowerState ROS message by parsing a RobotState message.
Expand Down
Loading

0 comments on commit 50916a5

Please sign in to comment.