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 
301  bool satisfiesVelocityBounds(const double* values, double margin = 0.0) const
302  {
303  return satisfiesVelocityBounds(values, variable_bounds_, margin);
304  }
305 
307  virtual bool satisfiesVelocityBounds(const double* values, const Bounds& other_bounds, double margin) const;
308 
310  bool enforceVelocityBounds(double* values) const
311  {
312  return enforceVelocityBounds(values, variable_bounds_);
313  }
314 
316  virtual bool enforceVelocityBounds(double* values, const Bounds& other_bounds) const;
317 
319  const VariableBounds& getVariableBounds(const std::string& variable) const;
320 
322  const Bounds& getVariableBounds() const
323  {
324  return variable_bounds_;
325  }
326 
328  void setVariableBounds(const std::string& variable, const VariableBounds& bounds);
329 
331  void setVariableBounds(const std::vector<moveit_msgs::JointLimits>& jlim);
332 
334  const std::vector<moveit_msgs::JointLimits>& getVariableBoundsMsg() const
335  {
336  return variable_bounds_msg_;
337  }
338 
342  virtual double distance(const double* value1, const double* value2) const = 0;
343 
346  double getDistanceFactor() const
347  {
348  return distance_factor_;
349  }
350 
353  void setDistanceFactor(double factor)
354  {
355  distance_factor_ = factor;
356  }
357 
359  virtual unsigned int getStateSpaceDimension() const = 0;
360 
362  const JointModel* getMimic() const
363  {
364  return mimic_;
365  }
366 
368  double getMimicOffset() const
369  {
370  return mimic_offset_;
371  }
372 
374  double getMimicFactor() const
375  {
376  return mimic_factor_;
377  }
378 
380  void setMimic(const JointModel* mimic, double factor, double offset);
381 
383  const std::vector<const JointModel*>& getMimicRequests() const
384  {
385  return mimic_requests_;
386  }
387 
389  void addMimicRequest(const JointModel* joint);
390  void addDescendantJointModel(const JointModel* joint);
391  void addDescendantLinkModel(const LinkModel* link);
392 
394  const std::vector<const LinkModel*>& getDescendantLinkModels() const
395  {
396  return descendant_link_models_;
397  }
398 
400  const std::vector<const JointModel*>& getDescendantJointModels() const
401  {
402  return descendant_joint_models_;
403  }
404 
406  const std::vector<const JointModel*>& getNonFixedDescendantJointModels() const
407  {
408  return non_fixed_descendant_joint_models_;
409  }
410 
412  bool isPassive() const
413  {
414  return passive_;
415  }
416 
417  void setPassive(bool flag)
418  {
419  passive_ = flag;
420  }
421 
426  virtual void interpolate(const double* from, const double* to, const double t, double* state) const = 0;
427 
429  virtual double getMaximumExtent(const Bounds& other_bounds) const = 0;
430 
431  double getMaximumExtent() const
432  {
433  return getMaximumExtent(variable_bounds_);
434  }
435 
440  virtual void computeTransform(const double* joint_values, Eigen::Affine3d& transf) const = 0;
441 
443  virtual void computeVariablePositions(const Eigen::Affine3d& transform, double* joint_values) const = 0;
444 
447 protected:
448  void computeVariableBoundsMsg();
449 
451  std::string name_;
452 
455 
457  std::vector<std::string> local_variable_names_;
458 
460  std::vector<std::string> variable_names_;
461 
464 
465  std::vector<moveit_msgs::JointLimits> variable_bounds_msg_;
466 
469  VariableIndexMap variable_index_map_;
470 
473 
476 
479 
482 
485 
487  std::vector<const JointModel*> mimic_requests_;
488 
490  std::vector<const LinkModel*> descendant_link_models_;
491 
493  std::vector<const JointModel*> descendant_joint_models_;
494 
497  std::vector<const JointModel*> non_fixed_descendant_joint_models_;
498 
500  bool passive_;
501 
504 
507 
510 };
511 
513 std::ostream& operator<<(std::ostream& out, const VariableBounds& b);
514 }
515 }
516 
517 #endif
double getMimicFactor() const
If mimicking a joint, this is the multiplicative factor for that joint&#39;s value.
Definition: joint_model.h:374
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:500
const std::string & getName() const
Get the name of the joint.
Definition: joint_model.h:131
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
Definition: joint_model.h:196
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:457
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:383
std::string name_
Name of the joint.
Definition: joint_model.h:451
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:497
Bounds variable_bounds_
The bounds for each variable (low, high) in the same order as variable_names_.
Definition: joint_model.h:463
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 JointModel * mimic_
The joint this one mimics (NULL for joints that do not mimic)
Definition: joint_model.h:478
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:346
JointType type_
The type of joint.
Definition: joint_model.h:454
bool hasVariable(const std::string &variable) const
Check if a particular variable is known to this joint.
Definition: joint_model.h:190
std::map< std::string, VariableBounds > VariableBoundsMap
Data type for holding mappings from variable names to their bounds.
Definition: joint_model.h:88
bool enforceVelocityBounds(double *values) const
Force the specified velocities to be within bounds. Return true if changes were made.
Definition: joint_model.h:310
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
double getMaximumExtent() const
Definition: joint_model.h:431
double mimic_offset_
The multiplier to the mimic joint.
Definition: joint_model.h:484
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:400
int getJointIndex() const
Get the index of this joint within the robot model.
Definition: joint_model.h:214
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:490
bool isPassive() const
Check if this joint is passive.
Definition: joint_model.h:412
std::map< std::string, JointModel * > JointModelMap
Map of names to instances for JointModel.
Definition: joint_model.h:91
double distance_factor_
The factor applied to the distance between two joint states.
Definition: joint_model.h:503
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
double getMimicOffset() const
If mimicking a joint, this is the offset added to that joint&#39;s value.
Definition: joint_model.h:368
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:322
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
VariableIndexMap variable_index_map_
Map from variable names to the corresponding index in variable_names_ (indexing makes sense within th...
Definition: joint_model.h:469
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
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:481
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:487
const LinkModel * parent_link_model_
The link before this joint.
Definition: joint_model.h:472
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
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:475
int getFirstVariableIndex() const
Get the index of this joint&#39;s first variable within the full robot state.
Definition: joint_model.h:202
const LinkModel * getChildLinkModel() const
Get the link that this joint connects to. There will always be such a link.
Definition: joint_model.h:154
int joint_index_
Index for this joint in the array of joints of the complete model.
Definition: joint_model.h:509
std::vector< moveit_msgs::JointLimits > variable_bounds_msg_
Definition: joint_model.h:465
std::vector< std::string > variable_names_
The full names to use for the variables that make up this joint.
Definition: joint_model.h:460
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
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
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:394
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:493
Main namespace for MoveIt!
void setParentLinkModel(const LinkModel *link)
Definition: joint_model.h:159
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:301
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
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
const std::vector< moveit_msgs::JointLimits > & getVariableBoundsMsg() const
Get the joint limits known to this model, as a message.
Definition: joint_model.h:334
void setPassive(bool flag)
Definition: joint_model.h:417
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:353
int first_variable_index_
The index of this joint&#39;s first variable, in the complete robot state.
Definition: joint_model.h:506
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 * > & getNonFixedDescendantJointModels() const
Get all the non-fixed joint models that descend from this joint, in the kinematic tree...
Definition: joint_model.h:406
const JointModel * getMimic() const
Get the joint this one is mimicking.
Definition: joint_model.h:362


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jan 15 2018 03:50:44