From 2c7aa3330a72f45f1c8526b5da5d706b7726bcc0 Mon Sep 17 00:00:00 2001
From: Francois Bleibel <fbleibel@gmail.com>
Date: Mon, 21 Jun 2010 16:02:56 +0900
Subject: [PATCH] Added forgotten cpp files.
---
src/math/op-point-modifier.cpp | 114 +++++++++++++++++++++++++++++++++
src/sot/sot-h.cpp | 4 +-
2 files changed, 116 insertions(+), 2 deletions(-)
create mode 100644 src/math/op-point-modifier.cpp
diff --git a/src/math/op-point-modifier.cpp b/src/math/op-point-modifier.cpp
new file mode 100644
index 00000000..8c4fbdf1
--- /dev/null
+++ b/src/math/op-point-modifier.cpp
@@ -0,0 +1,114 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: sotOpPointModifior.cp
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+#include <dynamic-graph/all-signals.h>
+
+#include <sot-core/op-point-modifier.h>
+#include <sot-core/matrix-twist.h>
+
+#include <dynamic-graph/pool.h>
+#include <sot-core/factory.h>
+
+using namespace std;
+using namespace sot;
+
+namespace sot {
+SOT_FACTORY_ENTITY_PLUGIN(sotOpPointModifior,"OpPointModifior");
+}
+
+
+/* --------------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+sotOpPointModifior::
+sotOpPointModifior( const std::string& name )
+ :Entity( name )
+ ,transformation()
+ ,jacobianSIN(NULL,"OpPointModifior("+name+")::input(matrix)::jacobianIN")
+ ,positionSIN(NULL,"OpPointModifior("+name+")::input(matrixhomo)::positionIN")
+ ,jacobianSOUT( boost::bind(&sotOpPointModifior::computeJacobian,this,_1,_2),
+ jacobianSIN,
+ "OpPointModifior("+name+")::output(matrix)::jacobian" )
+ ,positionSOUT( boost::bind(&sotOpPointModifior::computePosition,this,_1,_2),
+ jacobianSIN,
+ "OpPointModifior("+name+")::output(matrixhomo)::position" )
+
+
+{
+ signalRegistration( jacobianSIN<<positionSIN<<jacobianSOUT<<positionSOUT );
+ sotDEBUGINOUT(15);
+}
+
+ml::Matrix&
+sotOpPointModifior::computeJacobian( ml::Matrix& res,const int& time )
+{
+ const ml::Matrix& jacobian = jacobianSIN( time );
+ sotMatrixTwist V( transformation );
+ res = V*jacobian;
+ return res;
+}
+
+sotMatrixHomogeneous&
+sotOpPointModifior::computePosition( sotMatrixHomogeneous& res,const int& time )
+{
+ sotDEBUGIN(15);
+ sotDEBUGIN(15) << time << " " << positionSIN.getTime() << positionSOUT.getTime() << endl;
+ const sotMatrixHomogeneous& position = positionSIN( time );
+ position.multiply(transformation,res);
+ sotDEBUGOUT(15);
+ return res;
+}
+
+void
+sotOpPointModifior::setTransformation( const sotMatrixHomogeneous& tr )
+{ transformation = tr; }
+
+
+void sotOpPointModifior::
+commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os )
+{
+ if( cmdLine == "transfo" )
+ {
+ sotMatrixHomogeneous tr;
+ cmdArgs >> tr;
+ setTransformation(tr);
+ }
+ else if( cmdLine == "transfoSignal" )
+ {
+ Signal< sotMatrixHomogeneous,int > &sig
+ = dynamic_cast< Signal< sotMatrixHomogeneous,int >& >
+ (pool.getSignal( cmdArgs ));
+ setTransformation(sig.accessCopy());
+ }
+ else if( cmdLine == "getTransfo" )
+ {
+ os << "Transformation: " << endl << transformation <<endl;
+ }
+ else if( cmdLine == "help" )
+ {
+ os << "sotOpPointModifior"<<endl
+ << " - transfo MatrixHomo"<<endl
+ << " - transfoSignal ent.signal<matrixhomo>"<<endl
+ << " - getTransfo"<<endl;
+ }
+ else Entity::commandLine(cmdLine,cmdArgs,os);
+}
diff --git a/src/sot/sot-h.cpp b/src/sot/sot-h.cpp
index 500e1941..23a8180a 100644
--- a/src/sot/sot-h.cpp
+++ b/src/sot/sot-h.cpp
@@ -31,14 +31,14 @@
using namespace std;
using namespace sot;
-#ifdef VP_DEBUG
+//#ifdef VP_DEBUG
class sotSOTH__INIT
{
public:sotSOTH__INIT( void ) { sot::DebugTrace::openFile(); }
};
sotSOTH__INIT sotSOTH_initiator;
-#endif
+//#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
--
GitLab