RobotIK.h
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2015-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package RobotComponents::ArmarXObjects::RobotIK
19  * @author Joshua Haustein ( joshua dot haustein at gmail dot com )
20  * @date 2015
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #pragma once
26 
30 
32 #include <RobotComponents/interface/components/RobotIK.h>
33 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
34 #include <RobotAPI/interface/core/RobotState.h>
35 
36 #include <VirtualRobot/IK/GenericIKSolver.h>
37 
38 #include <mutex>
39 
40 namespace armarx
41 {
42  /**
43  * \class RobotIKPropertyDefinition
44  * \brief
45  */
48  {
49  public:
52  {
53  defineRequiredProperty<std::string>("RobotFileName", "Filename of VirtualRobot robot model (e.g. robot_model.xml)");
54  //Define optional properties
55  defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the robot state component that should be used");
56  defineOptionalProperty<std::string>("ReachabilitySpacesFolder", "Path to a folder containing reachability spaces");
57  defineOptionalProperty<int>("NumIKTrials", 10, "Number of trials to find an ik solution");
58  defineOptionalProperty<std::string>("InitialReachabilitySpaces", "", "Reachability spaces to load initially (semi-colon separated)");
59  }
60  };
61 
62  /**
63  * \defgroup Component-RobotIK RobotIK
64  * \ingroup RobotComponents-Components
65  * \brief Provides IK solving methods from VirtualRobot (see [Simox](http://simox.sourceforge.net/)).
66  *
67  * RobotIK provides a set of functions that allow computing IKs for any kinematic chain
68  * in a robot. This component requires the following properties:
69  * - RobotFileName: The VirtualRobot robot model to be used.
70  * - RobotStateComponentName: The name of a robot state component.
71  *
72  * \image html Armar3_IK_small.png "An exemplary IK task and its solution."
73  *
74  * Furthermore, there are the following optional properties:
75  * - ReachabilitySpacesFolder: A Path to a folder containing reachability spaces.
76  * - NumIKTrials: The number of trials the underlying ik solver tries to find a solution before giving up (default: 10).
77  *
78  * The robot model is required to solve the IK. Some of the provided functionalities require the current robot state,
79  * which is retrieved from the robot state component specified in the properties. These functions will only work properly,
80  * if the robot model used by the robot state component is identical to the robot model of this component.
81  *
82  * The functionalities include:
83  * - computation of an IK solution for a kinematic chain given a goal TCP pose.
84  * - computation of an IK solution for a set of robot joints such that
85  * the projection of the center of mass to the support surface lies within a goal region.
86  * - computation of a joint value gradient to minimize the workspace error for several tasks simultaneously.
87  * - creation and query of reachability spaces for kinematic chains.
88  *
89  * A reachability space for a kinematic chain allows a fast check whether
90  * a TCP pose is reachable. Creating a reachability space, however, is a computational intensive process
91  * that should be done offline. You may create a reachability space by calling the respective function
92  * or by specifying a path to a folder, ReachabilitySpacesFolder, that contains a set of reachability spaces.
93  * If ReachabilitySpacesFolder is specified, each file in the folder is attempted to be loaded as reachability space
94  * during initialization.
95  *
96  * While this implementation allows parallel access to its interface functions,
97  * it is not optimized towards maximizing parallelism.
98  * If you need to access this component from a large number of threads/processes
99  * and you care about performance, you should consider creating multiple instances
100  * of this component or using local instances of the IK solver from Simox instead.
101  *
102  * Furthermore, at the beginning of an operation this component synchronizes its
103  * internal robot state with that provided by the robot state component.
104  * This can lead to invalid IK solutions if the robot state changes while computation is still in progress.
105  * For example, imagine you compute an IK solution just
106  * for the forearm and the wrist given a global eef pose. If during the computation the configuration
107  * of the elbow changes, the computed ik solution will no longer achieve the desired eef pose.
108  *
109  * Also see [Simox-Tutorial] (http://sourceforge.net/p/simox/wiki/VirtualRobot/) for more information
110  * of how to use Simox for IK solving.
111  */
112 
113  /**
114  * @ingroup Component-RobotIK
115  * @brief Refer to \ref Component-RobotIK
116  */
118  virtual public Component,
119  virtual public RobotIKInterface
120  {
121  public:
122 
123  /**
124  * \return the robot xml filename as specified in the configuration
125  */
126  virtual std::string getRobotFilename(const Ice::Current&) const;
127 
128  /**
129  * Create an instance of RobotIKPropertyDefinitions.
130  */
131  PropertyDefinitionsPtr createPropertyDefinitions() override;
132 
133  std::string getDefaultName() const override
134  {
135  return "RobotIK";
136  }
137 
138  /**
139  * @brief Computes a single IK solution for the given robot node set and desired TCP pose.
140  * @details The TCP pose can be defined in any frame and is converted internally to a global pose
141  * according to the current robot state. The CartesianSelection
142  * parameter defines which part of the target pose should be reached.
143  *
144  * @param robotNodeSetName The name of a robot node set (robot node set) that is either stored
145  * within the robot model or has been defined via \ref defineRobotNodeSet.
146  * @param tcpPose The framed target pose for the TCP.
147  * @param cs Specifies which part of the pose needs to be reached by the IK, e.g. the position only,
148  * the orientation only or all.
149  * @return A map that maps each joint name to its value in the found IK solution.
150  */
151  NameValueMap computeIKFramedPose(const std::string& robotNodeSetName,
152  const FramedPoseBasePtr& tcpPose, armarx::CartesianSelection cs, const Ice::Current& = Ice::emptyCurrent) override;
153 
154  /**
155  * @brief Computes a single IK solution for the given robot node set and desired global TCP pose.
156  * @details The TCP pose is assumed to be in the global frame. The CartesianSelection
157  * parameter defines which part of the target pose should be reached.
158  *
159  * @param robotNodeSetName The name of a robot node set (robot node set) that is either stored
160  * within the robot model or has been defined via \ref defineRobotNodeSet.
161  * @param tcpPose The global target pose for the TCP.
162  * @param cs Specifies which part of the pose needs to be reached by the IK, e.g. the position only,
163  * the orientation only or all.
164  * @return A map that maps each joint name to its value in the found IK solution.
165  */
166  NameValueMap computeIKGlobalPose(const std::string& robotNodeSetName,
167  const PoseBasePtr& tcpPose, armarx::CartesianSelection cs, const Ice::Current& = Ice::emptyCurrent) override;
168 
169  /**
170  * @brief Computes a single IK solution, error and reachability for the given robot node set and desired global TCP pose.
171  * @details The TCP pose is assumed to be in the global frame. The CartesianSelection
172  * parameter defines which part of the target pose should be reached.
173  *
174  * @param robotNodeSetName The name of a robot node set (robot node set) that is either stored
175  * within the robot model or has been defined via \ref defineRobotNodeSet.
176  * @param tcpPose The global target pose for the TCP.
177  * @param cs Specifies which part of the pose needs to be reached by the IK, e.g. the position only,
178  * the orientation only or all.
179  * @return A map that maps each joint name to its value in the found IK solution, the reachability and computational error.
180  */
181  ExtendedIKResult computeExtendedIKGlobalPose(const std::string& robotNodeSetName,
182  const PoseBasePtr& tcpPose, armarx::CartesianSelection cs, const Ice::Current& = Ice::emptyCurrent) override;
183 
184  /**
185  * @brief Computes an IK solution for the given robot joints such that the center of mass lies above the
186  * given point.
187  * @details
188  *
189  * @param robotNodeSetJoints Name of the robot node set that contains the joints you wish to compute the IK for.
190  * @param comIK A center of mass description. Note that the priority field is relevant for this function as there
191  * is only a single CoM of descriptor.
192  *
193  * @return The ik-solution. Returns an empty vector if there is no solution.
194  */
195  NameValueMap computeCoMIK(const std::string& robotNodeSetJoints, const CoMIKDescriptor& desc, const Ice::Current& = Ice::emptyCurrent) override;
196 
197  /**
198  * @brief Computes a configuration gradient in order to solve several tasks/constraints simultaneously.
199  * @details This function allows you to use the HierarchicalIK solver provided by Simox.
200  * It computes a configuration gradient for the given robot node set that minimizes the workspace errors
201  * for multiple tasks simultaneously. You can specify IK tasks, a center of mass task and a joint limit avoidance task.
202  * For details for the different type of tasks, see the parameter description. You must define a priority for each task.
203  * The task with maximal priority is the first task to be solved. Each subsequent task is then solved
204  * within the null space of the previous task. <b> Note that this function returns a gradient and NOT
205  * an absolute IK solution. The gradient is computed on the current configuration of the robot. <\b>.
206  * The gradient is computed from the current robot configuration.
207  *
208  * See @url http://simox.sourceforge.net/documentation/class_virtual_robot_1_1_hierarchical_i_k.html
209  * and @url http://sourceforge.net/p/simox/wiki/VirtualRobot/#hierarchical-ik-solving for more information.
210  *
211  * @param robotNodeSet The robot node set (e.g. kinematic tree) you wish to compute the gradient for.
212  * @param diffIKs A list of IK tasks. Each IK task specifies a TCP and a desired pose for this TCP.
213  * @param comIK A center of mass tasks. Defines where the center should be and its priority. Is only used if the
214  CoM-task is enabled.
215  * @param stepSize
216  * @param avoidJointLimits Set whether or not to avoid joint limits.
217  * @param enableCenterOfMass Set whether or not to adjust the center of mass.
218  * @return A configuration gradient...
219  */
220  NameValueMap computeHierarchicalDeltaIK(const std::string& robotNodeSet,
221  const IKTasks& iktasks, const CoMIKDescriptor& comIK,
222  float stepSize, bool avoidJointLimits, bool enableCenterOfMass, const Ice::Current& = Ice::emptyCurrent) override;
223 
224  /**
225  * @brief Creates a new reachability space for the given robot node set.
226  * @details If there is no reachability space for the given robot node set yet, a new one is created. This may take
227  * some time. The function returns true iff a reachability space for the given robot node set exists after
228  * execution of this function.
229  *
230  * @param chainName The name of a defined robot node set. This can be either a robot node set that is defined in the
231  * robot model or a robot node set that has been manually defined calling \ref defineRobotNodeSet.
232  * @param coordinateSystem The name of the robot node in whose coordinate system the reachability space shall be defined
233  * in. If you wish to choose the global coordinate system, pass an empty string.
234  * Note, however, that any reachability space defined in the
235  * global coordinate system gets invalidated once the robot moves.
236  * @param stepTranslation The extend of a voxel dimension in translational dimensions (x,y,z) [mm]
237  * @param stepRotation The extend of a voxel dimension in rotational dimensions (roll,pitch,yaw) [rad]
238  * @param minBounds The minimum workspace poses (x,y,z,ro,pi,ya) given in the base node's coordinate system [mm and rad]
239  * @param maxBounds The maximum workspace poses (x,y,z,ro,pi,ya) given in base node's coordinate system [mm and rad]
240  * @param numSamples The number of tcp samples to take to create the reachability space (e.g 1000000)
241  * @return True iff the a reachability space for the given robot node set is available after execution of this function.
242  * False in case of a failure, e.g. there is no chain with the given name.
243  */
244  bool createReachabilitySpace(const std::string& chainName, const std::string& coordinateSystem, float stepTranslation, float stepRotation,
245  const WorkspaceBounds& minBounds, const WorkspaceBounds& maxBounds, int numSamples, const Ice::Current& = Ice::emptyCurrent) override;
246 
247  /**
248  * @brief Defines a new robot node set.
249  * @details Define a new robot node set with the given name that consists out of the given list of nodes with given
250  * TCP and root frame. Iff the chain is successfully added or already exists, <it>true</it> is returned.
251  * @param name The name of the robot node set.
252  * @param nodes The list of robot nodes that make up the robot node set.
253  * @param tcpName The name of the TCP node.
254  * @param rootNode The name of the kinematic root.
255  *
256  * @return True, iff chain was added or already exists. False, iff a different chain with similar name already exists.
257  */
258  bool defineRobotNodeSet(const std::string& name, const NodeNameList& nodes,
259  const std::string& tcpName, const std::string& rootNode, const Ice::Current& = Ice::emptyCurrent) override;
260 
261  /**
262  * @brief Returns whether this component has a reachability space for the given robot node set.
263  * @details True if there is a reachability space available, else false.
264  *
265  * @param chainName Name of the robot node set.
266  * @return True if there is a reachability space available, else false.
267  */
268  bool hasReachabilitySpace(const std::string& chainName, const Ice::Current& = Ice::emptyCurrent) const override;
269 
270  /**
271  * @brief Returns whether a given framed pose is currently reachable by the TCP of the given robot node set.
272  * @details To determine whether a pose is reachable a reachability space for the given robot node set is
273  * required. If no such space exists, the function returns <it>false<\it>.
274  * Call \ref createReachabilitySpace first to ensure there is such a space.
275  *
276  * @param chainName A name of a robot node set either defined in the robot model or previously defined via
277  * \ref defineRobotNodeSet.
278  * @param framedPose A framed pose to check for reachability. The pose is transformed into a global pose using the
279  * current robot state.
280  *
281  * @return True, if the pose is reachable by the TCP of the given chain. False, if it is not reachable or
282  * there is no reachability space for the given chain available.
283  */
284  bool isFramedPoseReachable(const std::string& chainName, const FramedPoseBasePtr& tcpPose, const Ice::Current& = Ice::emptyCurrent) const override;
285 
286  /**
287  * @brief Returns whether a given global pose is currently reachable by the TCP of the given robot node set.
288  * @details To determine whether a pose is reachable a reachability space for the given robot node set is
289  * required. If no such space exists, the function returns <it>false<\it>.
290  * Call \ref createReachabilitySpace first to ensure there is such a space.
291  *
292  * @param chainName A name of a robot node set either defined in the robot model or previously defined via
293  * \ref defineRobotNodeSet.
294  * @param pose A global pose to check for reachability.
295  *
296  * @return True, if the pose is reachable by the TCP of the given chain. False, if it is not reachable or
297  * there is no reachability space for the given chain available.
298  */
299  bool isPoseReachable(const std::string& chainName, const PoseBasePtr& tcpPose, const Ice::Current& = Ice::emptyCurrent) const override;
300 
301  /**
302  * @brief Loads the reachability space from the given file.
303  * @details If loading the reachability space succeeds, the reachability space is ready to be used after this function
304  * terminates. A reachability space can only be loaded if there is no reachability space for the respective
305  * robot node set yet.
306  *
307  * @param filename Binary file containing a reachability space. Ensure that the path is valid and accessible from
308  * where this component is running.
309  * @return True iff loading the reachability space was successful.
310  */
311  bool loadReachabilitySpace(const std::string& filename, const Ice::Current& = Ice::emptyCurrent) override;
312 
313  /**
314  * @brief Saves a previously created reachability space of the given robot node set.
315  * @details A reachability space for a robot node set can only be saved, if actually is one.
316  * You can check whether a reachability space is available by calling \ref hasReachabilitySpace(robotNodeSet).
317  *
318  * @param robotNodeSet The robot node set for which you wish to save the reachability space.
319  * @param filename The filename if the file(must be an accessible path for this component) you wish to save the space in.
320  * @return True iff saving was successful.
321  */
322  bool saveReachabilitySpace(const std::string& robotNodeSet, const std::string& filename, const Ice::Current& = Ice::emptyCurrent) const override;
323 
324 
325  protected:
326  /**
327  * Load and create a VirtualRobot::Robot instance from the RobotFileName
328  * property.
329  */
330  void onInitComponent() override;
331  void onConnectComponent() override;
332  void onDisconnectComponent() override;
333 
334  private:
335  bool solveIK(const Eigen::Matrix4f& tcpPose, armarx::CartesianSelection cs, VirtualRobot::RobotNodeSetPtr nodeSet);
336  void computeIK(const std::string& robotNodeSetName, const Eigen::Matrix4f& tcpPose,
337  armarx::CartesianSelection cs, NameValueMap& iksolution);
338 
339  void computeIK(VirtualRobot::RobotNodeSetPtr nodeSet, const Eigen::Matrix4f& tcpPose,
340  armarx::CartesianSelection cs, NameValueMap& iksolution);
341 
342  void computeIK(const std::string& robotNodeSetName, const Eigen::Matrix4f& tcpPose,
343  armarx::CartesianSelection cs, ExtendedIKResult& iksolution);
344 
345  void computeIK(VirtualRobot::RobotNodeSetPtr nodeSet, const Eigen::Matrix4f& tcpPose,
346  armarx::CartesianSelection cs, ExtendedIKResult& iksolution);
347 
348  Eigen::Matrix4f toGlobalPose(const FramedPoseBasePtr& tcpPose) const;
349  Eigen::Matrix4f toReachabilityMapFrame(const FramedPoseBasePtr& tcpPose, const std::string& chainName) const;
350 
351  bool isReachable(const std::string& setName, const Eigen::Matrix4f& tcpPose) const;
352 
354 
355  VirtualRobot::JacobiProvider::InverseJacobiMethod convertInverseJacobiMethod(armarx::InverseJacobiMethod aenum) const;
356 
357  void synchRobot() const;
358 
359  // Lock this mutex if you plan to modify the robot model
360  // Exception: When adding node sets we don't need to.
361  // Invariant: We never delete node sets nor nodes!
362  mutable std::recursive_mutex _modifyRobotModelMutex;
363  // Lock this mutex if you are adding a node set, to ensure we do not add
364  // a similar node set at the same time.
365  mutable std::recursive_mutex _editRobotNodeSetsMutex;
366  // Lock this mutex if you are reading or editing the reachability cache.
367  mutable std::recursive_mutex _accessReachabilityCacheMutex;
368  // Needs to be locked when computing IKs! Internally thread safe though.
369  VirtualRobot::RobotPtr _robotModel;
370  std::string _robotFile;
371  RobotStateComponentInterfacePrx _robotStateComponentPrx;
372  SharedRobotInterfacePrx _synchronizedRobot;
373  DebugDrawerInterfacePrx debugDrawer;
374 
375  // Reachability cache: not thread safe, always lock!
376  using ReachabilityCacheType = std::map<std::string, VirtualRobot::WorkspaceRepresentationPtr>;
377  ReachabilityCacheType _reachabilities;
378  int _numIKTrials;
379  };
380 
381 }
382 
Properties.h
armarx::PropertyDefinitionContainer::prefix
std::string prefix
Prefix of the properties such as namespace, domain, component name, etc.
Definition: PropertyDefinitionContainer.h:333
armarx::navigation::platform_controller::platform_global_trajectory::NameValueMap
std::map< std::string, float > NameValueMap
Definition: PlatformGlobalTrajectoryController.h:93
RobotAPIObjectFactories.h
armarx::RobotIK
Refer to RobotIK.
Definition: RobotIK.h:117
ARMARXCOMPONENT_IMPORT_EXPORT
#define ARMARXCOMPONENT_IMPORT_EXPORT
Definition: ImportExportComponent.h:38
armarx::RobotIKPropertyDefinitions
Definition: RobotIK.h:46
armarx::NJointTaskSpaceDMPControllerMode::CartesianSelection
CartesianSelection
Definition: ControllerInterface.ice:34
filename
std::string filename
Definition: VisualizationRobot.cpp:84
Component.h
armarx::Component
Baseclass for all ArmarX ManagedIceObjects requiring properties.
Definition: Component.h:95
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
armarx::RobotIKPropertyDefinitions::RobotIKPropertyDefinitions
RobotIKPropertyDefinitions(std::string prefix)
Definition: RobotIK.h:50
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:70
IceUtil::Handle< class PropertyDefinitionContainer >
IceInternal::ProxyHandle<::IceProxy::armarx::RobotStateComponentInterface >
armarx::RobotIK::getDefaultName
std::string getDefaultName() const override
Retrieve default name of component.
Definition: RobotIK.h:133
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:18
ImportExportComponent.h