prismatic_joint_model.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
38 #include <limits>
39 
40 namespace moveit
41 {
42 namespace core
43 {
44 PrismaticJointModel::PrismaticJointModel(const std::string& name) : JointModel(name), axis_(0.0, 0.0, 0.0)
45 {
46  type_ = PRISMATIC;
47  variable_names_.push_back(name_);
48  variable_bounds_.resize(1);
49  variable_bounds_[0].position_bounded_ = true;
50  variable_bounds_[0].min_position_ = -std::numeric_limits<double>::max();
51  variable_bounds_[0].max_position_ = std::numeric_limits<double>::max();
54 }
55 
57 {
58  return 1;
59 }
60 
61 double PrismaticJointModel::getMaximumExtent(const Bounds& other_bounds) const
62 {
63  return variable_bounds_[0].max_position_ - other_bounds[0].min_position_;
64 }
65 
66 void PrismaticJointModel::getVariableDefaultPositions(double* values, const Bounds& bounds) const
67 {
68  // if zero is a valid value
69  if (bounds[0].min_position_ <= 0.0 && bounds[0].max_position_ >= 0.0)
70  values[0] = 0.0;
71  else
72  values[0] = (bounds[0].min_position_ + bounds[0].max_position_) / 2.0;
73 }
74 
75 bool PrismaticJointModel::satisfiesPositionBounds(const double* values, const Bounds& bounds, double margin) const
76 {
77  return !(values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin);
78 }
79 
81  const Bounds& bounds) const
82 {
83  values[0] = rng.uniformReal(bounds[0].min_position_, bounds[0].max_position_);
84 }
85 
87  const Bounds& bounds, const double* near,
88  const double distance) const
89 {
90  values[0] = rng.uniformReal(std::max(bounds[0].min_position_, near[0] - distance),
91  std::min(bounds[0].max_position_, near[0] + distance));
92 }
93 
94 bool PrismaticJointModel::enforcePositionBounds(double* values, const Bounds& bounds) const
95 {
96  if (values[0] < bounds[0].min_position_)
97  {
98  values[0] = bounds[0].min_position_;
99  return true;
100  }
101  else if (values[0] > bounds[0].max_position_)
102  {
103  values[0] = bounds[0].max_position_;
104  return true;
105  }
106  return false;
107 }
108 
109 double PrismaticJointModel::distance(const double* values1, const double* values2) const
110 {
111  return fabs(values1[0] - values2[0]);
112 }
113 
114 void PrismaticJointModel::interpolate(const double* from, const double* to, const double t, double* state) const
115 {
116  state[0] = from[0] + (to[0] - from[0]) * t;
117 }
118 
119 void PrismaticJointModel::computeTransform(const double* joint_values, Eigen::Affine3d& transf) const
120 {
121  double* d = transf.data();
122  d[0] = 1.0;
123  d[1] = 0.0;
124  d[2] = 0.0;
125  d[3] = 0.0;
126 
127  d[4] = 0.0;
128  d[5] = 1.0;
129  d[6] = 0.0;
130  d[7] = 0.0;
131 
132  d[8] = 0.0;
133  d[9] = 0.0;
134  d[10] = 1.0;
135  d[11] = 0.0;
136 
137  d[12] = axis_.x() * joint_values[0];
138  d[13] = axis_.y() * joint_values[0];
139  d[14] = axis_.z() * joint_values[0];
140  d[15] = 1.0;
141 
142  // transf.setIdentity();
143  // transf.translation() = Eigen::Vector3d(axis_ * joint_values[0]);
144 }
145 
146 void PrismaticJointModel::computeVariablePositions(const Eigen::Affine3d& transf, double* joint_values) const
147 {
148  joint_values[0] = transf.translation().dot(axis_);
149 }
150 
151 } // end of namespace core
152 } // end of namespace moveit
d
virtual void interpolate(const double *from, const double *to, const double t, double *state) const
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
virtual void computeVariablePositions(const Eigen::Affine3d &transf, double *joint_values) const
Given the transform generated by joint, compute the corresponding joint values.
std::string name_
Name of the joint.
Definition: joint_model.h:451
Bounds variable_bounds_
The bounds for each variable (low, high) in the same order as variable_names_.
Definition: joint_model.h:463
JointType type_
The type of joint.
Definition: joint_model.h:454
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PrismaticJointModel(const std::string &name)
double getMaximumExtent() const
Definition: joint_model.h:431
Eigen::Vector3d axis_
The axis of the joint.
virtual unsigned int getStateSpaceDimension() const
Get the dimension of the state space that corresponds to this joint.
virtual bool satisfiesPositionBounds(const double *values, const Bounds &other_bounds, double margin) const
Check if the set of position values for the variables of this joint are within bounds, up to some margin.
virtual void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, double *values, const Bounds &other_bounds, const double *near, const double distance) const
Provide random values for the joint variables (within specified bounds). Enough memory is assumed to ...
double uniformReal(double lower_bound, double upper_bound)
virtual void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values, const Bounds &other_bounds) const
Provide random values for the joint variables (within specified bounds). Enough memory is assumed to ...
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
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
Definition: joint_model.h:108
virtual bool enforcePositionBounds(double *values, const Bounds &other_bounds) const
Force the specified values to be inside bounds and normalized. Quaternions are normalized, continuous joints are made between -Pi and Pi. Return true if changes were made.
std::vector< std::string > variable_names_
The full names to use for the variables that make up this joint.
Definition: joint_model.h:460
virtual void getVariableDefaultPositions(double *values, const Bounds &other_bounds) const
Provide a default value for the joint given the joint variable bounds. Most joints will use the defau...
virtual void computeTransform(const double *joint_values, Eigen::Affine3d &transf) const
Given the joint values for a joint, compute the corresponding transform.
Main namespace for MoveIt!
std::vector< VariableBounds > Bounds
The datatype for the joint bounds.
Definition: joint_model.h:123
virtual double distance(const double *values1, const double *values2) const
Compute the distance between two joint states of the same model (represented by the variable values) ...


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Oct 18 2020 13:16:33