Kindr
Kinematics and Dynamics for Robotics
Wrench.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  * * Redistributions of source code must retain the above copyright
8  * notice, this list of conditions and the following disclaimer.
9  * * Redistributions in binary form must reproduce the above copyright
10  * notice, this list of conditions and the following disclaimer in the
11  * documentation and/or other materials provided with the distribution.
12  * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
13  * names of its contributors may be used to endorse or promote products
14  * derived from this software without specific prior written permission.
15  *
16  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19  * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
20  * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
21  * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
22  * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
23  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
24  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26  *
27 */
28 /*
29  * Wernch.hpp
30  *
31  * Created on: Jan 31, 2017
32  * Author: Christian Gehring
33  */
34 
35 #pragma once
36 
39 
40 namespace kindr {
41 
43 template <typename PrimType_>
44 class Wrench6 : public WrenchBase<Wrench6<PrimType_>> {
45 public:
46  typedef PrimType_ Scalar;
49  typedef Eigen::Matrix<PrimType_, 6, 1> Vector6;
50  typedef Eigen::Matrix<PrimType_, 3, 1> Vector3;
51 
52  explicit Wrench6() : force_(Force::Zero()), torque_(Torque::Zero()) {
53 
54  }
55 
56  explicit Wrench6(const Force& force, const Torque& torque) :
57  force_(force),
58  torque_(torque) {
59  }
60 
61  explicit Wrench6(const Vector3& force, const Vector3& torque) :
62  force_(Force(force)),
63  torque_(Torque(torque)) {
64  }
65 
66  explicit Wrench6(const Vector6& wrench) :
67  force_(Force(wrench.head(3))),
68  torque_(Torque(wrench.tail(3))) {
69  }
70 
71  virtual ~Wrench6() {
72 
73  }
74 
75  inline Force & getForce() {
76  return force_;
77  }
78 
79  inline const Force & getForce() const {
80  return force_;
81  }
82 
83  inline Torque & getTorque() {
84  return torque_;
85  }
86 
87  inline const Torque & getTorque() const {
88  return torque_;
89  }
90 
91  inline void setForce(const Force& force) {
92  force_ = force;
93  }
94 
95  inline void setTorque(const Torque& torque) {
96  torque_ = torque;
97  }
98 
99  inline void setForce(const Vector3& force) {
100  force_ = Force(force);
101  }
102 
103  inline void setTorque(const Vector3& torque) {
104  torque_ = Torque(torque);
105  }
106 
107  inline void setVector(const Vector6& wrench) {
108  *this = Wrench6(wrench);
109  }
110 
111  inline Vector6 getVector() const {
112  Vector6 vector;
113  vector.template head<3>() = getForce().toImplementation();
114  vector.template tail<3>() = getTorque().toImplementation();
115  return vector;
116  }
117 
119  force_.setZero();
120  torque_.setZero();
121  return *this;
122  }
123 
128  Wrench6 & operator=(const Wrench6& other) {
129  this->getForce() = other.getForce();
130  this->getTorque() = other.getTorque();
131  return *this;
132  }
133 
138  Wrench6 operator+(const Wrench6& other) const {
139  return Wrench6(this->getForce() + other.getForce(), this->getTorque() + other.getTorque());
140  }
141 
146  Wrench6 operator-(const Wrench6& other) const {
147  return Wrench6(this->getForce() - other.getForce(), this->getTorque() - other.getTorque());
148  }
149 
154  template<typename PrimTypeFactor_>
155  Wrench6 operator*(PrimTypeFactor_ factor) const {
156  return Wrench6(this->getForce()*(PrimType_)factor, this->getTorque()*(PrimType_)factor);
157  }
158 
163  template<typename PrimTypeDivisor_>
164  Wrench6 operator/(PrimTypeDivisor_ divisor) const {
165  return Wrench6(this->getForce()/(PrimType_)divisor, this->getTorque()/(PrimType_)divisor);
166  }
167 
172  Wrench6& operator+=(const Wrench6& other) {
173  this->getForce() += other.getForce();
174  this->getTorque() += other.getTorque();
175  return *this;
176  }
177 
182  Wrench6& operator-=(const Wrench6& other) {
183  this->getForce() -= other.getForce();
184  this->getTorque() -= other.getTorque();
185  return *this;
186  }
187 
192  template<typename PrimTypeFactor_>
193  Wrench6& operator*=(PrimTypeFactor_ factor) {
194  this->getForce() *= (PrimType_)factor;
195  this->getTorque() *= (PrimType_)factor;
196  return *this;
197  }
198 
203  template<typename PrimTypeDivisor_>
204  Wrench6& operator/=(PrimTypeDivisor_ divisor) {
205  this->getForce() /= (PrimType_)divisor;
206  this->getTorque() /= (PrimType_)divisor;
207  return *this;
208  }
209 
213  Wrench6 operator-() const {
214  return Wrench6(-this->getForce(), -this->getTorque());
215  }
216 
221  bool operator==(const Wrench6& other) const {
222  return ((this->getForce() == other.getForce()) && (this->getTorque() == other.getTorque()));
223  }
224 
225 
229  friend std::ostream & operator << (std::ostream & out, const Wrench6 & wrench) {
230  out << wrench.getForce() << " " << wrench.getTorque();
231  return out;
232  }
233 protected:
234  Force force_;
235  Torque torque_;
236 };
237 
240 
241 
242 } // namespace kindr
Wrench6< double > WrenchD
Definition: Wrench.hpp:238
Wrench6 operator*(PrimTypeFactor_ factor) const
Multiplies vector with a scalar.
Definition: Wrench.hpp:155
Wrench6 & setZero()
Definition: Wrench.hpp:118
Wrench6 operator/(PrimTypeDivisor_ divisor) const
Divides vector by a scalar.
Definition: Wrench.hpp:164
Wrench6 & operator*=(PrimTypeFactor_ factor)
Multiplication with a scalar and assignment.
Definition: Wrench.hpp:193
void setForce(const Force &force)
Definition: Wrench.hpp:91
Wrench6(const Vector3 &force, const Vector3 &torque)
Definition: Wrench.hpp:61
Wrench6 & operator-=(const Wrench6 &other)
Subtraction and assignment of two vectors.
Definition: Wrench.hpp:182
Torque & getTorque()
Definition: Wrench.hpp:83
void setTorque(const Vector3 &torque)
Definition: Wrench.hpp:103
void setTorque(const Torque &torque)
Definition: Wrench.hpp:95
void setForce(const Vector3 &force)
Definition: Wrench.hpp:99
Vector< PhysicalType::Force, PrimType_, 3 > Force
Definition: Wrench.hpp:47
Eigen::Matrix< PrimType_, 3, 1 > Vector3
Definition: Wrench.hpp:50
virtual ~Wrench6()
Definition: Wrench.hpp:71
Wrench6(const Vector6 &wrench)
Definition: Wrench.hpp:66
bool operator==(const Wrench6 &other) const
Comparison operator.
Definition: Wrench.hpp:221
Force force_
Definition: Wrench.hpp:234
friend std::ostream & operator<<(std::ostream &out, const Wrench6 &wrench)
Used for printing the object with std::cout.
Definition: Wrench.hpp:229
const Torque & getTorque() const
Definition: Wrench.hpp:87
Eigen::Matrix< PrimType_, 6, 1 > Vector6
Definition: Wrench.hpp:49
const Force & getForce() const
Definition: Wrench.hpp:79
Wrench6 operator-() const
Negation of a vector.
Definition: Wrench.hpp:213
Vector< PhysicalType::Torque, PrimType_, 3 > Torque
Definition: Wrench.hpp:48
Wrench6 & operator=(const Wrench6 &other)
Assignment operator.
Definition: Wrench.hpp:128
void setVector(const Vector6 &wrench)
Definition: Wrench.hpp:107
Wrench6 & operator+=(const Wrench6 &other)
Addition and assignment of two vectors.
Definition: Wrench.hpp:172
Force & getForce()
Definition: Wrench.hpp:75
PrimType_ Scalar
Definition: Wrench.hpp:46
Wrench6(const Force &force, const Torque &torque)
Definition: Wrench.hpp:56
Wrench6 operator-(const Wrench6 &other) const
Subtraction of two vectors.
Definition: Wrench.hpp:146
Torque torque_
Definition: Wrench.hpp:235
Vector< PhysicalType_, PrimType_, Dimension_ > & setZero()
Sets all components of the vector to zero.
Definition: Vector.hpp:136
Wrench6 operator+(const Wrench6 &other) const
Addition of two vectors.
Definition: Wrench.hpp:138
Implementation & toImplementation()
Cast to the implementation type.
Definition: Vector.hpp:302
Wrench6 & operator/=(PrimTypeDivisor_ divisor)
Division by a scalar and assignment.
Definition: Wrench.hpp:204
Wrench6< float > WrenchF
Definition: Wrench.hpp:239
Vector6 getVector() const
Definition: Wrench.hpp:111