Go to the documentation of this file.
49 const Vector& _V_base_center,
50 const Vector& V_base_p,
51 const Rotation& R_base_end,
53 RotationalInterpolation* _orient,
60 F_base_center.p = _V_base_center;
61 orient->SetStartEnd(F_base_start.M,R_base_end);
62 double oalpha = orient->Angle();
64 Vector x(F_base_start.p - F_base_center.p);
65 radius = x.Normalize();
69 throw Error_MotionPlanning_Circle_ToSmall();
71 Vector tmpv(V_base_p-F_base_center.p);
74 double n = z.Normalize();
78 throw Error_MotionPlanning_Circle_No_Plane();
80 F_base_center.M = Rotation(x,z*x,z);
81 double dist = alpha*radius;
86 if (oalpha*eqradius > dist) {
88 pathlength = oalpha*eqradius;
89 scalerot = 1/eqradius;
90 scalelin = dist/pathlength;
94 scalerot = oalpha/pathlength;
virtual void Write(std::ostream &os)
virtual Vector Vel(double theta, double thetad) const =0
Vector p
origine of the Frame
virtual Twist Vel(double s, double sd) const
virtual Twist Acc(double s, double sd, double sdd) const
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
const double deg2rad
the value pi/180
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
virtual void Write(std::ostream &os) const =0
RotationalInterpolation * orient
virtual Vector Acc(double theta, double thetad, double thetadd) const =0
virtual double PathLength()
Vector UnitY() const
Access to the underlying unitvectors of the rotation matrix.
Rotation M
Orientation of the Frame.
Path_Circle(const Frame &F_base_start, const Vector &V_base_center, const Vector &V_base_p, const Rotation &R_base_end, double alpha, RotationalInterpolation *otraj, double eqradius, bool _aggregate=true)
virtual RotationalInterpolation * Clone() const =0
double LengthToS(double length)
virtual Frame Pos(double s) const
virtual Rotation Pos(double theta) const =0