From b5ca6be46a199f79c2ab6063d0ab6b0f136d6dfd Mon Sep 17 00:00:00 2001
From: Syrine <boudsyrine@hotmail.com>
Date: Fri, 27 May 2011 20:02:22 +0200
Subject: [PATCH] Add external object door
---
src/CMakeLists.txt | 1 +
src/ZMPRefTrajectoryGeneration/door.cpp | 71 +++++++++++
src/ZMPRefTrajectoryGeneration/door.hh | 153 ++++++++++++++++++++++++
3 files changed, 225 insertions(+)
create mode 100644 src/ZMPRefTrajectoryGeneration/door.cpp
create mode 100644 src/ZMPRefTrajectoryGeneration/door.hh
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 21a225f4..055d2b9a 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -60,6 +60,7 @@ SET(SOURCES
ZMPRefTrajectoryGeneration/qp-problem.cpp
ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
ZMPRefTrajectoryGeneration/mpc-trajectory-generation.cpp
+ ZMPRefTrajectoryGeneration/door.cpp
MotionGeneration/StepOverPlanner.cpp
MotionGeneration/CollisionDetector.cpp
MotionGeneration/WaistHeightVariation.cpp
diff --git a/src/ZMPRefTrajectoryGeneration/door.cpp b/src/ZMPRefTrajectoryGeneration/door.cpp
new file mode 100644
index 00000000..2cd1b76b
--- /dev/null
+++ b/src/ZMPRefTrajectoryGeneration/door.cpp
@@ -0,0 +1,71 @@
+/*
+ * door.cpp
+ *
+ * Created on: 10 mai 2011
+ * Author: syrine
+ */
+
+#include <ZMPRefTrajectoryGeneration/door.hh>
+
+using namespace std;
+
+
+Door::Door()
+{}
+
+
+void
+Door::initialize( double Orientation)
+{
+
+ FrameBase_.Orientation_ = Orientation;
+
+ FrameBase_.InvRx_.resize(2,false);
+ FrameBase_.InvRx_(0) = cos(Orientation);
+ FrameBase_.InvRx_(1) = -sin(Orientation);
+ FrameBase_.InvRy_.resize(2,false);
+ FrameBase_.InvRy_(0) = sin(Orientation);
+ FrameBase_.InvRy_(1) = cos(Orientation);
+
+ double xp = FrameBase_.Position(X);
+ double yp = FrameBase_.Position(Y);
+ double zp = FrameBase_.Position(Z);
+ FrameBase_.InvHp_ = -FrameBase_.InvRx_*xp-FrameBase_.InvRy_*yp;
+
+}
+
+
+
+void
+Door::build_door_matrix(double Time, double DesVelDoor, int N, boost_ublas::matrix<double> & R, int RelativAngle, double & Xdoortip, double & Ydoortip )
+{
+
+ double Theta;
+ for(int j=0;j<N;j++)
+ {
+ Theta = DesVelDoor*(Time+j)+ RelativAngle;
+ R(0,j) = sin(Theta);
+ R(1,j) = -cos(Theta);
+ }
+
+// Rrot(0) = sin(DesVelDoor*(Time)+ RelativAngle);
+// Rrot(1) = -cos(DesVelDoor*(Time)+ RelativAngle);
+// // L largeur de la porte
+
+ double L = 1.0;
+ // door tip position in global frame
+ Xdoortip = L*sin( DesVelDoor*(Time))*cos(FrameBase_.Orientation_)- L*cos( DesVelDoor*(Time))*sin(FrameBase_.Orientation_)+FrameBase_.Position(X);
+ Ydoortip = L*sin( DesVelDoor*(Time))*sin(FrameBase_.Orientation_)+ L*cos( DesVelDoor*(Time))*cos(FrameBase_.Orientation_)+FrameBase_.Position(Y);
+ cout<<"FrameBase_.Orientation_"<<FrameBase_.Orientation_<< endl;
+}
+
+
+Door::frame_s::frame_s()
+{
+ Orientation_ = -3.14/2.0;
+ dOrientation_ = 0.0;
+ ddOrientation_ = 0.0;
+ Position_[X] = 0.55;
+ Position_[Y] = 0.5;
+ Position_[Z] = 0.0;
+}
diff --git a/src/ZMPRefTrajectoryGeneration/door.hh b/src/ZMPRefTrajectoryGeneration/door.hh
new file mode 100644
index 00000000..84a53db6
--- /dev/null
+++ b/src/ZMPRefTrajectoryGeneration/door.hh
@@ -0,0 +1,153 @@
+/*
+ * door.hh
+ *
+ * Created on: 10 mai 2011
+ * Author: syrine
+ */
+
+#ifndef DOOR_HH_
+#define DOOR_HH_
+
+#include <jrl/mal/matrixabstractlayer.hh>
+
+class Door
+{
+
+ //
+ // Public types:
+ //
+public:
+ const static unsigned int X = 0;
+ const static unsigned int Y = 1;
+ const static unsigned int Z = 2;
+
+ //
+ // Public methods:
+ //
+public:
+
+ Door(); //constructeur
+
+ void initialize( double Orientation);
+
+ ///
+ void build_door_matrix(double Time, double DesVelDoor, int N, boost_ublas::matrix<double> & R, int RelativAngle, double & Xdoortip, double & Ydoortip);
+ /// void doormatrices_construct();
+
+ struct frame_s
+ {
+
+ //
+ // Public methods
+ //
+ public:
+
+ frame_s();// constructeur de la structure
+
+ inline double const & Position(unsigned int Axis) const
+ { return Position_[Axis]; };
+ inline void Position( unsigned int Axis, double Position )
+ { Position_[Axis] = Position; };
+
+ inline double const Orientation() const
+ { return Orientation_; };
+ inline void Orientation( const double Orientation )
+ { Orientation_ = Orientation; };
+
+ inline double const dOrientation() const
+ { return dOrientation_; };
+ inline void dOrientation( const double dOrientation )
+ { dOrientation_ = dOrientation; };
+
+ inline double const ddOrientation() const
+ { return ddOrientation_; };
+ inline void ddOrientation( const double ddOrientation )
+ { ddOrientation_ = ddOrientation; };
+
+ //TODO:
+ // inline boost_ublas::vector<double> const Position() const
+ //{ return Postion_[3]; };
+ //inline void Postion( const boost_ublas::vector<double> Postion_[3] )
+ //{ Postion_[3] = Postion_[3]; };
+//
+// inline boost_ublas::matrix<double> const H() const
+// { return H; };
+// inline void H( const boost_ublas::matrix<double> H )
+// { H = H_; };
+//
+// inline boost_ublas::matrix<double> const Hinv() const
+// { return Hinv; };
+// inline void Hinv( const boost_ublas::matrix<double> Hinv )
+// { Hinv = Hinv; };
+
+ /// \brief Orientation around the vertical axis
+ double Orientation_;
+ // \brief Rotational velocity around the vertical axis
+ double dOrientation_;
+ // \brief Rotational acceleration around the vertical axis
+ double ddOrientation_;
+
+ /// \brief Position of the origin
+ double Position_[3];
+// /// \brief transformation matrix from
+// boost_ublas::matrix<double> H_;
+// /// \brief Inverse of H
+// boost_ublas::matrix<double> Hinv_;
+
+ /// \brief x rotation vector of the inv. transformation matrix InvH
+ boost_ublas::vector<double> InvRx_;
+ /// \brief y rotation vector of the inv. transformation matrix InvH
+ boost_ublas::vector<double> InvRy_;
+ /// \brief Translation component vector of InvH matrix
+ boost_ublas::vector<double> InvHp_;
+
+ };
+ typedef frame_s frame_t;
+
+ /// Definition des matrices correspondant à la porte
+
+
+ boost_ublas::vector<double> & InvRx()
+ { return FrameBase_.InvRx_; };
+ boost_ublas::vector<double> & InvRy()
+ { return FrameBase_.InvRy_; };
+ boost_ublas::vector<double> & InvHp()
+ { return FrameBase_.InvHp_; };
+
+
+
+ /// \name Matrices defining the evolution
+ /// \{
+// inline double const Mass() const
+// { return Mass_; };
+// inline void Mass( const double Mass )
+// { Mass_ = Mass; };
+//
+// inline double const Inertia() const
+// { return Inertia_; };
+// inline void Inertia( const double Mass )
+// { Inertia_ = Inertia; };
+ /// \}
+
+
+ //
+ // Private members:
+ //
+private:
+
+ /// Frame of the door
+ frame_t FrameBase_;
+ /// Frame of the door
+ frame_t FrameDoor_;
+// /// Mass
+// double Mass_;
+// /// Inertia
+// double Inertia_;
+ /// Wrist
+ /// TODO: Wrist undefined
+ frame_t FrameWrist_;
+
+
+};
+
+#endif /* DOOR_HH_ */
--
GitLab