Adds xpp_anymal

robots/xpp_anymal/CMakeLists.txt

@@ -0,0 +1,54 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(xpp_anymal)
+
+add_compile_options(-std=c++11)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  xpp_vis
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package()
+
+
+###########
+## Build ##
+###########
+## Specify additional locations of header files
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+# Declare a C++ library
+add_library(${PROJECT_NAME}
+  src/anymalleg_inverse_kinematics.cc
+  src/inverse_kinematics_anymal.cc
+)
+
+## URDF visualizer
+add_executable(urdf_visualizer_anymal src/exe/urdf_visualizer_anymal.cc)
+target_link_libraries(urdf_visualizer_anymal
+  ${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+# Mark library for installation
+install(
+  TARGETS ${PROJECT_NAME} urdf_visualizer_anymal
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+# Mark other files for installation
+install(
+  DIRECTORY launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

robots/xpp_anymal/include/xpp_anymal/anymalleg_inverse_kinematics.h

@@ -0,0 +1,76 @@
+/******************************************************************************
+Copyright (c) 2017, Alexander W. Winkler. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#ifndef XPP_VIS_ANYMALLEG_INVERSE_KINEMATICS_H_
+#define XPP_VIS_ANYMALLEG_INVERSE_KINEMATICS_H_
+
+#include <Eigen/Dense>
+
+namespace xpp {
+
+enum ANYmalJointID {HAA=0, HFE, KFE, ANYmallegJointCount};
+
+/**
+ * @brief Converts a anymal foot position to joint angles.
+ */
+class ANYmallegInverseKinematics {
+public:
+  using Vector3d = Eigen::Vector3d;
+  enum KneeBend { Forward, Backward };
+
+  /**
+   * @brief Default c'tor initializing leg lengths with standard values.
+   */
+  ANYmallegInverseKinematics () = default;
+  virtual ~ANYmallegInverseKinematics () = default;
+
+  /**
+   * @brief Returns the joint angles to reach a Cartesian foot position.
+   * @param ee_pos_H  Foot position xyz expressed in the frame attached
+   * at the hip-aa (H).
+   */
+  Vector3d GetJointAngles(const Vector3d& ee_pos_H, KneeBend bend=Forward) const;
+
+  /**
+   * @brief Restricts the joint angles to lie inside the feasible range
+   * @param q[in/out]  Current joint angle that is adapted if it exceeds
+   * the specified range.
+   * @param joint  Which joint (HAA, HFE, KFE) this value represents.
+   */
+  void EnforceLimits(double& q, ANYmalJointID joint) const;
+
+private:
+  Vector3d hfe_to_haa_z = Vector3d(0.0, 0.0, 0.0); //distance of HFE to HAA in z direction
+  double length_thigh = 0.27; // length of upper leg
+  double length_shank = 0.29; // length of lower leg
+};
+
+} /* namespace xpp */
+
+#endif /* XPP_VIS_ANYMALLEG_INVERSE_KINEMATICS_H_ */

robots/xpp_anymal/include/xpp_anymal/inverse_kinematics_anymal.h

@@ -0,0 +1,64 @@
+/******************************************************************************
+Copyright (c) 2017, Alexander W. Winkler. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#ifndef XPP_VIS_INVERSEKINEMATICS_ANYMAL_H_
+#define XPP_VIS_INVERSEKINEMATICS_ANYMAL_H_
+
+#include <xpp_vis/inverse_kinematics.h>
+#include <xpp_anymal/anymalleg_inverse_kinematics.h>
+
+namespace xpp {
+
+/**
+ * @brief Inverse kinematics function for the ANYmal robot.
+ */
+class InverseKinematicsANYmal : public InverseKinematics {
+public:
+  InverseKinematicsANYmal() = default;
+  virtual ~InverseKinematicsANYmal() = default;
+
+  /**
+   * @brief Returns joint angles to reach for a specific foot position.
+   * @param pos_B  3D-position of the foot expressed in the base frame (B).
+   */
+  Joints GetAllJointAngles(const EndeffectorsPos& pos_b) const override;
+
+  /**
+   * @brief Number of endeffectors (feet, hands) this implementation expects.
+   */
+  int GetEECount() const override { return 4; };
+
+private:
+  Vector3d base2hip_LF_ = Vector3d(0.2770,  0.1160, 0.0);
+  ANYmallegInverseKinematics leg;
+};
+
+} /* namespace xpp */
+
+#endif /* XPP_VIS_INVERSEKINEMATICS_ANYMAL_H_ */

robots/xpp_anymal/launch/anymal.launch

@@ -0,0 +1,9 @@
+<launch>
+
+    <!-- Upload URDF file to ros parameter server for rviz to find  -->
+    <param name="anymal_rviz_urdf_robot_description" textfile="$(find anymal_b_simple_description)/urdf/anymal.urdf"/>
+
+    <!-- Start tf visualizer  -->
+    <node name="urdf_visualizer_anymal" pkg="xpp_anymal" type="urdf_visualizer_anymal" output="screen"/>
+
+</launch>

robots/xpp_anymal/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+
+<package format="2">
+    <name>xpp_anymal</name>
+    <version>1.0.0</version>
+    <description>
+        ANYmal-specific functions for visualization in the  XPP Motion Framework.
+    </description>
+
+    <author>Alexander W. Winkler</author>
+    <author email="henrique.ferrolho@ed.ac.uk">Henrique Ferrolho</author>
+    <maintainer email="alexander.w.winkler@gmail.com">Alexander W. Winkler</maintainer>
+    <license>BSD</license>
+
+    <url type="website">http://github.com/leggedrobotics/xpp</url>
+    <url type="bugtracker">http://github.com/leggedrobotics/xpp/issues</url>
+
+    <buildtool_depend>catkin</buildtool_depend>
+    <depend>xacro</depend>
+    <depend>roscpp</depend>
+    <depend>xpp_vis</depend>
+</package>

robots/xpp_anymal/src/anymalleg_inverse_kinematics.cc

@@ -0,0 +1,152 @@
+/******************************************************************************
+Copyright (c) 2017, Alexander W. Winkler. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#include <xpp_anymal/anymalleg_inverse_kinematics.h>
+
+#include <cmath>
+#include <map>
+
+#include <xpp_states/cartesian_declarations.h>
+
+
+namespace xpp {
+
+
+ANYmallegInverseKinematics::Vector3d
+ANYmallegInverseKinematics::GetJointAngles (const Vector3d& ee_pos_B, KneeBend bend) const
+{
+  double q_HAA_bf, q_HAA_br, q_HFE_br; // rear bend of knees
+  double q_HFE_bf, q_KFE_br, q_KFE_bf; // forward bend of knees
+
+  Eigen::Vector3d xr;
+  Eigen::Matrix3d R;
+
+  // translate to the local coordinate of the attachment of the leg
+  // and flip coordinate signs such that all computations can be done
+  // for the front-left leg
+  xr = ee_pos_B;
+
+  // compute the HAA angle
+  q_HAA_bf = q_HAA_br = -atan2(xr[Y],-xr[Z]);
+
+  // rotate into the HFE coordinate system (rot around X)
+  R << 1.0, 0.0, 0.0, 0.0, cos(q_HAA_bf), -sin(q_HAA_bf), 0.0, sin(q_HAA_bf), cos(q_HAA_bf);
+
+  xr = (R * xr).eval();
+
+  // translate into the HFE coordinate system (along Z axis)
+  xr += hfe_to_haa_z;  //distance of HFE to HAA in z direction
+
+  // compute square of length from HFE to foot
+  double tmp1 = pow(xr[X],2)+pow(xr[Z],2);
+
+
+  // compute temporary angles (with reachability check)
+  double lu = length_thigh;  // length of upper leg
+  double ll = length_shank;  // length of lower leg
+  double alpha = atan2(-xr[Z],xr[X]) - 0.5*M_PI;  //  flip and rotate to match HyQ joint definition
+
+
+  double some_random_value_for_beta = (pow(lu,2)+tmp1-pow(ll,2))/(2.*lu*sqrt(tmp1)); // this must be between -1 and 1
+  if (some_random_value_for_beta > 1) {
+    some_random_value_for_beta = 1;
+  }
+  if (some_random_value_for_beta < -1) {
+    some_random_value_for_beta = -1;
+  }
+  double beta = acos(some_random_value_for_beta);
+
+  // compute Hip FE angle
+  q_HFE_bf = q_HFE_br = alpha + beta;
+
+
+  double some_random_value_for_gamma = (pow(ll,2)+pow(lu,2)-tmp1)/(2.*ll*lu);
+  // law of cosines give the knee angle
+  if (some_random_value_for_gamma > 1) {
+    some_random_value_for_gamma = 1;
+  }
+  if (some_random_value_for_gamma < -1) {
+    some_random_value_for_gamma = -1;
+  }
+  double gamma  = acos(some_random_value_for_gamma);
+
+
+  q_KFE_bf = q_KFE_br = gamma - M_PI;
+
+  // forward knee bend
+  EnforceLimits(q_HAA_bf, HAA);
+  EnforceLimits(q_HFE_bf, HFE);
+  EnforceLimits(q_KFE_bf, KFE);
+
+  // backward knee bend
+  EnforceLimits(q_HAA_br, HAA);
+  EnforceLimits(q_HFE_br, HFE);
+  EnforceLimits(q_KFE_br, KFE);
+
+  if (bend==Forward)
+    return Vector3d(q_HAA_bf, q_HFE_bf, q_KFE_bf);
+  else // backward
+    return Vector3d(q_HAA_br, -q_HFE_br, -q_KFE_br);
+}
+
+void
+ANYmallegInverseKinematics::EnforceLimits (double& val, ANYmalJointID joint) const
+{
+  // totally exaggerated joint angle limits
+  const static double haa_min = -180;
+  const static double haa_max =  90;
+
+  const static double hfe_min = -90;
+  const static double hfe_max =  90;
+
+  const static double kfe_min = -180;
+  const static double kfe_max =  0;
+
+  // reduced joint angles for optimization
+  static const std::map<ANYmalJointID, double> max_range {
+    {HAA, haa_max/180.0*M_PI},
+    {HFE, hfe_max/180.0*M_PI},
+    {KFE, kfe_max/180.0*M_PI}
+  };
+
+  // reduced joint angles for optimization
+  static const std::map<ANYmalJointID, double> min_range {
+    {HAA, haa_min/180.0*M_PI},
+    {HFE, hfe_min/180.0*M_PI},
+    {KFE, kfe_min/180.0*M_PI}
+  };
+
+  double max = max_range.at(joint);
+  val = val>max? max : val;
+
+  double min = min_range.at(joint);
+  val = val<min? min : val;
+}
+
+} /* namespace xpp */