GCC Code Coverage Report

Line	Exec	Source
1		/*
2		* Copyright 2010,
3		* François Bleibel,
4		* Olivier Stasse,
5		*
6		* CNRS/AIST
7		*
8		*/
9
10		#ifndef __SOT_SOTGRIPPERCONTROL_H__
11		#define __SOT_SOTGRIPPERCONTROL_H__
12
13		/* --------------------------------------------------------------------- */
14		/* --- INCLUDE --------------------------------------------------------- */
15		/* --------------------------------------------------------------------- */
16
17		/* Matrix */
18		#include <dynamic-graph/linear-algebra.h>
19
20		/* SOT */
21		#include <dynamic-graph/all-signals.h>
22		#include <dynamic-graph/entity.h>
23
24		#include <sot/core/flags.hh>
25
26		/* STD */
27		#include <string>
28
29		/* --------------------------------------------------------------------- */
30		/* --- API ------------------------------------------------------------- */
31		/* --------------------------------------------------------------------- */
32
33		#if defined(WIN32)
34		#if defined(gripper_control_EXPORTS)
35		#define SOTGRIPPERCONTROL_EXPORT __declspec(dllexport)
36		#else
37		#define SOTGRIPPERCONTROL_EXPORT __declspec(dllimport)
38		#endif
39		#else
40		#define SOTGRIPPERCONTROL_EXPORT
41		#endif
42
43		/* --------------------------------------------------------------------- */
44		/* --- CLASS ----------------------------------------------------------- */
45		/* --------------------------------------------------------------------- */
46
47		namespace dynamicgraph {
48		namespace sot {
49
50		/*! \brief The goal of this entity is to ensure that the maximal torque will not
51		* be exceeded during a grasping task.
52		* If the maximal torque is reached, then the current position of the gripper is
53		kept
54		*
55		*/
56		class SOTGRIPPERCONTROL_EXPORT GripperControl {
57		protected:
58		double offset;
59		static const double OFFSET_DEFAULT;
60		//! \brief The multiplication
61		dynamicgraph::Vector factor;
62
63		public:
64		GripperControl(void);
65
66		//! \brief Computes the
67		// if the torque limit is reached, the normalized position is reduced by
68		// (offset)
69		void computeIncrement(const dynamicgraph::Vector &torques,
70		const dynamicgraph::Vector &torqueLimits,
71		const dynamicgraph::Vector &currentNormVel);
72
73		//! \brief
74		dynamicgraph::Vector &computeDesiredPosition(
75		const dynamicgraph::Vector &currentPos,
76		const dynamicgraph::Vector &desiredPos,
77		const dynamicgraph::Vector &torques,
78		const dynamicgraph::Vector &torqueLimits,
79		dynamicgraph::Vector &referencePos);
80
81		/*! \brief select only some of the values of the vector fullsize,
82		* based on the Flags vector.
83		*/
84
85		static dynamicgraph::Vector &selector(const dynamicgraph::Vector &fullsize,
86		const Flags &selec,
87		dynamicgraph::Vector &desPos);
88		};
89
90		/* --------------------------------------------------------------------- */
91		/* --- PLUGIN ---------------------------------------------------------- */
92		/* --------------------------------------------------------------------- */
93
94		class SOTGRIPPERCONTROL_EXPORT GripperControlPlugin
95		: public dynamicgraph::Entity,
96		public GripperControl {
97	✗	DYNAMIC_GRAPH_ENTITY_DECL();
98
99		public:
100		bool calibrationStarted;
101
102		public: /* --- CONSTRUCTION --- */
103		GripperControlPlugin(const std::string &name);
104		virtual ~GripperControlPlugin(void);
105
106		/* --- DOCUMENTATION --- */
107		virtual std::string getDocString() const;
108
109		public: /* --- SIGNAL --- */
110		/* --- INPUTS --- */
111		dynamicgraph::SignalPtr<dynamicgraph::Vector, int> positionSIN;
112		dynamicgraph::SignalPtr<dynamicgraph::Vector, int> positionDesSIN;
113		dynamicgraph::SignalPtr<dynamicgraph::Vector, int> torqueSIN;
114		dynamicgraph::SignalPtr<dynamicgraph::Vector, int> torqueLimitSIN;
115		dynamicgraph::SignalPtr<Flags, int> selectionSIN;
116
117		/* --- INTERMEDIARY --- */
118		dynamicgraph::SignalPtr<dynamicgraph::Vector, int> positionFullSizeSIN;
119		dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int>
120		positionReduceSOUT;
121		dynamicgraph::SignalPtr<dynamicgraph::Vector, int> torqueFullSizeSIN;
122		dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> torqueReduceSOUT;
123		dynamicgraph::SignalPtr<dynamicgraph::Vector, int> torqueLimitFullSizeSIN;
124		dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int>
125		torqueLimitReduceSOUT;
126
127		/* --- OUTPUTS --- */
128		dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int>
129		desiredPositionSOUT;
130
131		public: /* --- COMMANDLINE --- */
132		void initCommands();
133
134		void setOffset(const double &value);
135		};
136
137		} /* namespace sot */
138		} /* namespace dynamicgraph */
139
140		#endif // #ifndef __SOT_SOTGRIPPERCONTROL_H__
141