From a914b02520fdf400beee17116a221a26839ae857 Mon Sep 17 00:00:00 2001
From: Henrique Ferrolho <henriqueferrolho@gmail.com>
Date: Mon, 20 Jul 2020 14:27:16 +0100
Subject: [PATCH 1/2] [WIP] Adds xpp_anymal
---
robots/xpp_anymal/CMakeLists.txt | 54 +++++++
.../xpp_anymal/anymalleg_inverse_kinematics.h | 76 +++++++++
.../xpp_anymal/inverse_kinematics_anymal.h | 64 ++++++++
robots/xpp_anymal/launch/anymal.launch | 9 ++
robots/xpp_anymal/package.xml | 22 +++
.../src/anymalleg_inverse_kinematics.cc | 152 ++++++++++++++++++
.../src/exe/urdf_visualizer_anymal.cc | 84 ++++++++++
.../src/inverse_kinematics_anymal.cc | 78 +++++++++
8 files changed, 539 insertions(+)
create mode 100644 robots/xpp_anymal/CMakeLists.txt
create mode 100644 robots/xpp_anymal/include/xpp_anymal/anymalleg_inverse_kinematics.h
create mode 100644 robots/xpp_anymal/include/xpp_anymal/inverse_kinematics_anymal.h
create mode 100644 robots/xpp_anymal/launch/anymal.launch
create mode 100644 robots/xpp_anymal/package.xml
create mode 100644 robots/xpp_anymal/src/anymalleg_inverse_kinematics.cc
create mode 100644 robots/xpp_anymal/src/exe/urdf_visualizer_anymal.cc
create mode 100644 robots/xpp_anymal/src/inverse_kinematics_anymal.cc
diff --git a/robots/xpp_anymal/CMakeLists.txt b/robots/xpp_anymal/CMakeLists.txt
new file mode 100644
index 0000000..e4a9af4
--- /dev/null
+++ b/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 rviz meshes urdf
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/robots/xpp_anymal/include/xpp_anymal/anymalleg_inverse_kinematics.h b/robots/xpp_anymal/include/xpp_anymal/anymalleg_inverse_kinematics.h
new file mode 100644
index 0000000..66e6b89
--- /dev/null
+++ b/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_ */
diff --git a/robots/xpp_anymal/include/xpp_anymal/inverse_kinematics_anymal.h b/robots/xpp_anymal/include/xpp_anymal/inverse_kinematics_anymal.h
new file mode 100644
index 0000000..76da98d
--- /dev/null
+++ b/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_ */
diff --git a/robots/xpp_anymal/launch/anymal.launch b/robots/xpp_anymal/launch/anymal.launch
new file mode 100644
index 0000000..5cff499
--- /dev/null
+++ b/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>
diff --git a/robots/xpp_anymal/package.xml b/robots/xpp_anymal/package.xml
new file mode 100644
index 0000000..a593a70
--- /dev/null
+++ b/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>
\ No newline at end of file
diff --git a/robots/xpp_anymal/src/anymalleg_inverse_kinematics.cc b/robots/xpp_anymal/src/anymalleg_inverse_kinematics.cc
new file mode 100644
index 0000000..9843861
--- /dev/null
+++ b/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 */
diff --git a/robots/xpp_anymal/src/exe/urdf_visualizer_anymal.cc b/robots/xpp_anymal/src/exe/urdf_visualizer_anymal.cc
new file mode 100644
index 0000000..65640f3
--- /dev/null
+++ b/robots/xpp_anymal/src/exe/urdf_visualizer_anymal.cc
@@ -0,0 +1,84 @@
+/******************************************************************************
+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 <iostream>
+#include <map>
+#include <memory>
+#include <string>
+
+#include <ros/init.h>
+
+#include <xpp_anymal/inverse_kinematics_anymal.h>
+#include <xpp_msgs/topic_names.h>
+#include <xpp_states/joints.h>
+#include <xpp_states/endeffector_mappings.h>
+
+#include <xpp_vis/cartesian_joint_converter.h>
+#include <xpp_vis/urdf_visualizer.h>
+
+using namespace xpp;
+using namespace quad;
+
+int main(int argc, char *argv[])
+{
+ ::ros::init(argc, argv, "anymal_urdf_visualizer");
+
+ const std::string joint_desired_anymal = "xpp/joint_anymal_des";
+
+ auto anymal_ik = std::make_shared<InverseKinematicsANYmal>();
+ CartesianJointConverter inv_kin_converter(anymal_ik,
+ xpp_msgs::robot_state_desired,
+ joint_desired_anymal);
+
+ // urdf joint names
+ int n_ee = anymal_ik->GetEECount();
+ int n_j = ANYmallegJointCount;
+ std::vector<UrdfVisualizer::URDFName> joint_names(n_ee*n_j);
+ joint_names.at(n_j*LF + HAA) = "LF_HAA";
+ joint_names.at(n_j*LF + HFE) = "LF_HFE";
+ joint_names.at(n_j*LF + KFE) = "LF_KFE";
+ joint_names.at(n_j*RF + HAA) = "RF_HAA";
+ joint_names.at(n_j*RF + HFE) = "RF_HFE";
+ joint_names.at(n_j*RF + KFE) = "RF_KFE";
+ joint_names.at(n_j*LH + HAA) = "LH_HAA";
+ joint_names.at(n_j*LH + HFE) = "LH_HFE";
+ joint_names.at(n_j*LH + KFE) = "LH_KFE";
+ joint_names.at(n_j*RH + HAA) = "RH_HAA";
+ joint_names.at(n_j*RH + HFE) = "RH_HFE";
+ joint_names.at(n_j*RH + KFE) = "RH_KFE";
+
+ std::string urdf = "anymal_rviz_urdf_robot_description";
+ UrdfVisualizer anymal_desired(urdf, joint_names, "base", "world",
+ joint_desired_anymal, "anymal_des");
+
+ ::ros::spin();
+
+ return 1;
+}
+
diff --git a/robots/xpp_anymal/src/inverse_kinematics_anymal.cc b/robots/xpp_anymal/src/inverse_kinematics_anymal.cc
new file mode 100644
index 0000000..0500cde
--- /dev/null
+++ b/robots/xpp_anymal/src/inverse_kinematics_anymal.cc
@@ -0,0 +1,78 @@
+/******************************************************************************
+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/inverse_kinematics_anymal.h>
+
+#include <xpp_states/cartesian_declarations.h>
+#include <xpp_states/endeffector_mappings.h>
+
+namespace xpp {
+
+Joints
+InverseKinematicsANYmal::GetAllJointAngles(const EndeffectorsPos& x_B) const
+{
+ Vector3d ee_pos_H; // foothold expressed in hip frame
+ std::vector<Eigen::VectorXd> q_vec;
+
+ // make sure always exactly 4 elements
+ auto pos_B = x_B.ToImpl();
+ pos_B.resize(4, pos_B.front());
+
+ for (int ee=0; ee<pos_B.size(); ++ee) {
+
+ ANYmallegInverseKinematics::KneeBend bend = ANYmallegInverseKinematics::Forward;
+
+ using namespace quad;
+ switch (ee) {
+ case LF:
+ ee_pos_H = pos_B.at(ee);
+ break;
+ case RF:
+ ee_pos_H = pos_B.at(ee).cwiseProduct(Eigen::Vector3d(1,-1,1));
+ break;
+ case LH:
+ ee_pos_H = pos_B.at(ee).cwiseProduct(Eigen::Vector3d(-1,1,1));
+ bend = ANYmallegInverseKinematics::Backward;
+ break;
+ case RH:
+ ee_pos_H = pos_B.at(ee).cwiseProduct(Eigen::Vector3d(-1,-1,1));
+ bend = ANYmallegInverseKinematics::Backward;
+ break;
+ default: // joint angles for this foot do not exist
+ break;
+ }
+
+ ee_pos_H -= base2hip_LF_;
+ q_vec.push_back(leg.GetJointAngles(ee_pos_H, bend));
+ }
+
+ return Joints(q_vec);
+}
+
+} /* namespace xpp */
From 5dc74aa6319b5d39816154c32e7962e0157784d6 Mon Sep 17 00:00:00 2001
From: Henrique Ferrolho <henriqueferrolho@gmail.com>
Date: Mon, 20 Jul 2020 18:04:14 +0100
Subject: [PATCH 2/2] Removes rviz meshes and urdf from install directories of
xpp_anymal
---
robots/xpp_anymal/CMakeLists.txt | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/robots/xpp_anymal/CMakeLists.txt b/robots/xpp_anymal/CMakeLists.txt
index e4a9af4..2a6df60 100644
--- a/robots/xpp_anymal/CMakeLists.txt
+++ b/robots/xpp_anymal/CMakeLists.txt
@@ -49,6 +49,6 @@ install(
# Mark other files for installation
install(
- DIRECTORY launch rviz meshes urdf
+ DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)