joint_model.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Ioan A. Sucan
5  * Copyright (c) 2013, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage, Inc. nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /* Author: Ioan Sucan */
37 
38 #ifndef MOVEIT_CORE_ROBOT_MODEL_JOINT_MODEL_
39 #define MOVEIT_CORE_ROBOT_MODEL_JOINT_MODEL_
40 
41 #include <string>
42 #include <vector>
43 #include <map>
44 #include <iostream>
45 #include <moveit_msgs/JointLimits.h>
47 #include <Eigen/Geometry>
48 
49 namespace moveit
50 {
51 namespace core
52 {
54 {
56  : min_position_(0.0)
57  , max_position_(0.0)
58  , position_bounded_(false)
59  , min_velocity_(0.0)
60  , max_velocity_(0.0)
61  , velocity_bounded_(false)
62  , min_acceleration_(0.0)
63  , max_acceleration_(0.0)
64  , acceleration_bounded_(false)
65  {
66  }
67 
68  double min_position_;
69  double max_position_;
71 
72  double min_velocity_;
73  double max_velocity_;
75 
79 };
80 
81 class LinkModel;
82 class JointModel;
83 
85 typedef std::map<std::string, int> VariableIndexMap;
86 
88 typedef std::map<std::string, VariableBounds> VariableBoundsMap;
89 
91 typedef std::map<std::string, JointModel*> JointModelMap;
92 
94 typedef std::map<std::string, const JointModel*> JointModelMapConst;
95 
109 {
110 public:
113  {
119  FIXED
120  };
121 
123  typedef std::vector<VariableBounds> Bounds;
124 
126  JointModel(const std::string& name);
127 
128  virtual ~JointModel();
129 
131  const std::string& getName() const
132  {
133  return name_;
134  }
135 
138  {
139  return type_;
140  }
141 
143  std::string getTypeName() const;
144 
149  {
150  return parent_link_model_;
151  }
152 
155  {
156  return child_link_model_;
157  }
158 
159  void setParentLinkModel(const LinkModel* link)
160  {
161  parent_link_model_ = link;
162  }
163 
164  void setChildLinkModel(const LinkModel* link)
165  {
166  child_link_model_ = link;
167  }
168 
176  const std::vector<std::string>& getVariableNames() const
177  {
178  return variable_names_;
179  }
180 
184  const std::vector<std::string>& getLocalVariableNames() const
185  {
186  return local_variable_names_;
187  }
188 
190  bool hasVariable(const std::string& variable) const
191  {
192  return variable_index_map_.find(variable) != variable_index_map_.end();
193  }
194 
196  std::size_t getVariableCount() const
197  {
198  return variable_names_.size();
199  }
200 
203  {
204  return first_variable_index_;
205  }
206 
208  void setFirstVariableIndex(int index)
209  {
210  first_variable_index_ = index;
211  }
212 
214  int getJointIndex() const
215  {
216  return joint_index_;
217  }
218 
220  void setJointIndex(int index)
221  {
222  joint_index_ = index;
223  }
224 
226  int getLocalVariableIndex(const std::string& variable) const;
227 
236  void getVariableDefaultPositions(double* values) const
237  {
238  getVariableDefaultPositions(values, variable_bounds_);
239  }
240 
244  virtual void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const = 0;
245 
249  {
250  getVariableRandomPositions(rng, values, variable_bounds_);
251  }
252 
255  virtual void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values,
256  const Bounds& other_bounds) const = 0;
257 
260  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, const double* near,
261  const double distance) const
262  {
263  getVariableRandomPositionsNearBy(rng, values, variable_bounds_, near, distance);
264  }
265 
268  virtual void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values,
269  const Bounds& other_bounds, const double* near,
270  const double distance) const = 0;
271 
278  bool satisfiesPositionBounds(const double* values, double margin = 0.0) const
279  {
280  return satisfiesPositionBounds(values, variable_bounds_, margin);
281  }
282 
285  virtual bool satisfiesPositionBounds(const double* values, const Bounds& other_bounds, double margin) const = 0;
286 
290  bool enforcePositionBounds(double* values) const
291  {
292  return enforcePositionBounds(values, variable_bounds_);
293  }
294 
298  virtual bool enforcePositionBounds(double* values, const Bounds& other_bounds) const = 0;
299 
302  virtual bool harmonizePosition(double* values, const Bounds& other_bounds) const;
303  bool harmonizePosition(double* values) const
304  {
305  return harmonizePosition(values, variable_bounds_);
306  }
307 
309  bool satisfiesVelocityBounds(const double* values, double margin = 0.0) const
310  {
311  return satisfiesVelocityBounds(values, variable_bounds_, margin);
312  }
313 
315  virtual bool satisfiesVelocityBounds(const double* values, const Bounds& other_bounds, double margin) const;
316 
318  bool enforceVelocityBounds(double* values) const
319  {
320  return enforceVelocityBounds(values, variable_bounds_);
321  }
322 
324  virtual bool enforceVelocityBounds(double* values, const Bounds& other_bounds) const;
325 
327  const VariableBounds& getVariableBounds(const std::string& variable) const;
328 
330  const Bounds& getVariableBounds() const
331  {
332  return variable_bounds_;
333  }
334 
336  void setVariableBounds(const std::string& variable, const VariableBounds& bounds);
337 
339  void setVariableBounds(const std::vector<moveit_msgs::JointLimits>& jlim);
340 
342  const std::vector<moveit_msgs::JointLimits>& getVariableBoundsMsg() const
343  {
344  return variable_bounds_msg_;
345  }
346 
350  virtual double distance(const double* value1, const double* value2) const = 0;
351 
354  double getDistanceFactor() const
355  {
356  return distance_factor_;
357  }
358 
361  void setDistanceFactor(double factor)
362  {
363  distance_factor_ = factor;
364  }
365 
367  virtual unsigned int getStateSpaceDimension() const = 0;
368 
370  const JointModel* getMimic() const
371  {
372  return mimic_;
373  }
374 
376  double getMimicOffset() const
377  {
378  return mimic_offset_;
379  }
380 
382  double getMimicFactor() const
383  {
384  return mimic_factor_;
385  }
386 
388  void setMimic(const JointModel* mimic, double factor, double offset);
389 
391  const std::vector<const JointModel*>& getMimicRequests() const
392  {
393  return mimic_requests_;
394  }
395 
397  void addMimicRequest(const JointModel* joint);
398  void addDescendantJointModel(const JointModel* joint);
399  void addDescendantLinkModel(const LinkModel* link);
400 
402  const std::vector<const LinkModel*>& getDescendantLinkModels() const
403  {
404  return descendant_link_models_;
405  }
406 
408  const std::vector<const JointModel*>& getDescendantJointModels() const
409  {
410  return descendant_joint_models_;
411  }
412 
414  const std::vector<const JointModel*>& getNonFixedDescendantJointModels() const
415  {
416  return non_fixed_descendant_joint_models_;
417  }
418 
420  bool isPassive() const
421  {
422  return passive_;
423  }
424 
425  void setPassive(bool flag)
426  {
427  passive_ = flag;
428  }
429 
434  virtual void interpolate(const double* from, const double* to, const double t, double* state) const = 0;
435 
437  virtual double getMaximumExtent(const Bounds& other_bounds) const = 0;
438 
439  double getMaximumExtent() const
440  {
441  return getMaximumExtent(variable_bounds_);
442  }
443 
448  virtual void computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const = 0;
449 
451  virtual void computeVariablePositions(const Eigen::Isometry3d& transform, double* joint_values) const = 0;
452 
455 protected:
456  void computeVariableBoundsMsg();
457 
459  std::string name_;
460 
463 
465  std::vector<std::string> local_variable_names_;
466 
468  std::vector<std::string> variable_names_;
469 
472 
473  std::vector<moveit_msgs::JointLimits> variable_bounds_msg_;
474 
477  VariableIndexMap variable_index_map_;
478 
481 
484 
487 
490 
493 
495  std::vector<const JointModel*> mimic_requests_;
496 
498  std::vector<const LinkModel*> descendant_link_models_;
499 
501  std::vector<const JointModel*> descendant_joint_models_;
502 
505  std::vector<const JointModel*> non_fixed_descendant_joint_models_;
506 
508  bool passive_;
509 
512 
515 
518 };
519 
521 std::ostream& operator<<(std::ostream& out, const VariableBounds& b);
522 }
523 }
524 
525 #endif
bool harmonizePosition(double *values) const
Definition: joint_model.h:303
void setChildLinkModel(const LinkModel *link)
Definition: joint_model.h:164
bool passive_
Specify whether this joint is marked as passive in the SRDF.
Definition: joint_model.h:508
JointType
The different types of joints we support.
Definition: joint_model.h:112
std::vector< std::string > local_variable_names_
The local names to use for the variables that make up this joint.
Definition: joint_model.h:465
const std::vector< const JointModel * > & getMimicRequests() const
The joint models whose values would be modified if the value of this joint changed.
Definition: joint_model.h:391
std::vector< double > values
int getJointIndex() const
Get the index of this joint within the robot model.
Definition: joint_model.h:214
bool isPassive() const
Check if this joint is passive.
Definition: joint_model.h:420
std::string name_
Name of the joint.
Definition: joint_model.h:459
void setJointIndex(int index)
Set the index of this joint within the robot model.
Definition: joint_model.h:220
std::vector< const JointModel * > non_fixed_descendant_joint_models_
Pointers to all the joints that follow this one in the kinematic tree, including mimic joints...
Definition: joint_model.h:505
Bounds variable_bounds_
The bounds for each variable (low, high) in the same order as variable_names_.
Definition: joint_model.h:471
void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, double *values, const double *near, const double distance) const
Provide random values for the joint variables (within default bounds). Enough memory is assumed to be...
Definition: joint_model.h:260
const std::vector< const JointModel * > & getDescendantJointModels() const
Get all the joint models that descend from this joint, in the kinematic tree.
Definition: joint_model.h:408
const JointModel * mimic_
The joint this one mimics (NULL for joints that do not mimic)
Definition: joint_model.h:486
JointType type_
The type of joint.
Definition: joint_model.h:462
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
Provide random values for the joint variables (within default bounds). Enough memory is assumed to be...
Definition: joint_model.h:248
bool satisfiesPositionBounds(const double *values, double margin=0.0) const
Check if the set of values for the variables of this joint are within bounds.
Definition: joint_model.h:278
const std::vector< const JointModel * > & getNonFixedDescendantJointModels() const
Get all the non-fixed joint models that descend from this joint, in the kinematic tree...
Definition: joint_model.h:414
const std::vector< std::string > & getLocalVariableNames() const
Get the local names of the variable that make up the joint (suffixes that are attached to joint names...
Definition: joint_model.h:184
std::map< std::string, VariableBounds > VariableBoundsMap
Data type for holding mappings from variable names to their bounds.
Definition: joint_model.h:88
geometry_msgs::TransformStamped t
double mimic_offset_
The multiplier to the mimic joint.
Definition: joint_model.h:492
const std::vector< std::string > & getVariableNames() const
Get the names of the variables that make up this joint, in the order they appear in corresponding sta...
Definition: joint_model.h:176
double getDistanceFactor() const
Get the factor that should be applied to the value returned by distance() when that value is used in ...
Definition: joint_model.h:354
bool satisfiesVelocityBounds(const double *values, double margin=0.0) const
Check if the set of velocities for the variables of this joint are within bounds. ...
Definition: joint_model.h:309
std::vector< const LinkModel * > descendant_link_models_
Pointers to all the links that will be moved if this joint changes value.
Definition: joint_model.h:498
bool enforcePositionBounds(double *values) const
Force the specified values to be inside bounds and normalized. Quaternions are normalized, continuous joints are made between -Pi and Pi. Returns true if changes were made.
Definition: joint_model.h:290
std::map< std::string, JointModel * > JointModelMap
Map of names to instances for JointModel.
Definition: joint_model.h:91
const std::vector< moveit_msgs::JointLimits > & getVariableBoundsMsg() const
Get the joint limits known to this model, as a message.
Definition: joint_model.h:342
double distance_factor_
The factor applied to the distance between two joint states.
Definition: joint_model.h:511
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
Definition: joint_model.h:196
const LinkModel * getChildLinkModel() const
Get the link that this joint connects to. There will always be such a link.
Definition: joint_model.h:154
void getVariableDefaultPositions(double *values) const
Provide a default value for the joint given the default joint variable bounds (maintained internally)...
Definition: joint_model.h:236
VariableIndexMap variable_index_map_
Map from variable names to the corresponding index in variable_names_ (indexing makes sense within th...
Definition: joint_model.h:477
std::map< std::string, int > VariableIndexMap
Data type for holding mappings from variable names to their position in a state vector.
Definition: joint_model.h:82
double mimic_factor_
The offset to the mimic joint.
Definition: joint_model.h:489
const Bounds & getVariableBounds() const
Get the variable bounds for this joint, in the same order as the names returned by getVariableNames()...
Definition: joint_model.h:330
double getMimicOffset() const
If mimicking a joint, this is the offset added to that joint&#39;s value.
Definition: joint_model.h:376
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
Definition: joint_model.h:108
std::vector< const JointModel * > mimic_requests_
The set of joints that should get a value copied to them when this joint changes. ...
Definition: joint_model.h:495
const LinkModel * parent_link_model_
The link before this joint.
Definition: joint_model.h:480
bool enforceVelocityBounds(double *values) const
Force the specified velocities to be within bounds. Return true if changes were made.
Definition: joint_model.h:318
void setFirstVariableIndex(int index)
Set the index of this joint&#39;s first variable within the full robot state.
Definition: joint_model.h:208
const LinkModel * child_link_model_
The link after this joint.
Definition: joint_model.h:483
bool hasVariable(const std::string &variable) const
Check if a particular variable is known to this joint.
Definition: joint_model.h:190
int joint_index_
Index for this joint in the array of joints of the complete model.
Definition: joint_model.h:517
std::vector< moveit_msgs::JointLimits > variable_bounds_msg_
Definition: joint_model.h:473
std::vector< std::string > variable_names_
The full names to use for the variables that make up this joint.
Definition: joint_model.h:468
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
int getFirstVariableIndex() const
Get the index of this joint&#39;s first variable within the full robot state.
Definition: joint_model.h:202
std::map< std::string, const JointModel * > JointModelMapConst
Map of names to const instances for JointModel.
Definition: joint_model.h:94
std::vector< const JointModel * > descendant_joint_models_
Pointers to all the joints that follow this one in the kinematic tree (including mimic joints) ...
Definition: joint_model.h:501
Main namespace for MoveIt!
void setParentLinkModel(const LinkModel *link)
Definition: joint_model.h:159
double getMaximumExtent() const
Definition: joint_model.h:439
double getMimicFactor() const
If mimicking a joint, this is the multiplicative factor for that joint&#39;s value.
Definition: joint_model.h:382
const std::vector< const LinkModel * > & getDescendantLinkModels() const
Get all the link models that descend from this joint, in the kinematic tree.
Definition: joint_model.h:402
std::ostream & operator<<(std::ostream &out, const VariableBounds &b)
Operator overload for printing variable bounds to a stream.
std::vector< VariableBounds > Bounds
The datatype for the joint bounds.
Definition: joint_model.h:123
const LinkModel * getParentLinkModel() const
Get the link that this joint connects to. The robot is assumed to start with a joint, so the root joint will return a NULL pointer here.
Definition: joint_model.h:148
JointType getType() const
Get the type of joint.
Definition: joint_model.h:137
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:55
void setPassive(bool flag)
Definition: joint_model.h:425
void setDistanceFactor(double factor)
Set the factor that should be applied to the value returned by distance() when that value is used in ...
Definition: joint_model.h:361
int first_variable_index_
The index of this joint&#39;s first variable, in the complete robot state.
Definition: joint_model.h:514
const JointModel * getMimic() const
Get the joint this one is mimicking.
Definition: joint_model.h:370
const std::string & getName() const
Get the name of the joint.
Definition: joint_model.h:131


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Dec 2 2019 03:56:58