Another set of UI messages fixes and tweaks! No functional changes.
authorBastien Montagne <montagne29@wanadoo.fr>
Sun, 23 Oct 2011 19:54:06 +0000 (19:54 +0000)
committerBastien Montagne <montagne29@wanadoo.fr>
Sun, 23 Oct 2011 19:54:06 +0000 (19:54 +0000)
GNUmakefile
SConstruct
intern/itasc/ConstraintSet.cpp
intern/itasc/ControlledObject.cpp
intern/itasc/CopyPose.cpp
intern/itasc/Scene.cpp
intern/itasc/WDLSSolver.cpp
intern/itasc/WSDLSSolver.cpp

index 5d1e5fe..b06344c 100644 (file)
@@ -1,6 +1,5 @@
 # -*- mode: gnumakefile; tab-width: 8; indent-tabs-mode: t; -*-
-# vim: tabstop=8
-# $Id: GNUmakefile 41122 2011-10-19 21:55:27Z campbellbarton $
+# vim: tabstop=4
 #
 # ##### BEGIN GPL LICENSE BLOCK #####
 #
index 0db3236..93787cd 100644 (file)
@@ -1,5 +1,5 @@
 #!/usr/bin/env python
-# $Id: SConstruct 41169 2011-10-21 04:23:26Z campbellbarton $
+#
 # ***** BEGIN GPL LICENSE BLOCK *****
 #
 # This program is free software; you can redistribute it and/or
@@ -30,6 +30,8 @@
 # Then read all SConscripts and build
 #
 # TODO: fix /FORCE:MULTIPLE on windows to get proper debug builds.
+# TODO: directory copy functions are far too complicated, see:
+#       http://wiki.blender.org/index.php/User:Ideasman42/SConsNotSimpleInstallingFiles
 
 import platform as pltfrm
 
index 74926df..e21cd36 100644 (file)
@@ -80,7 +80,7 @@ void ConstraintSet::modelUpdate(Frame& _external_pose,const Timestamp& timestamp
 
 double ConstraintSet::getMaxTimestep(double& timestep)
 {
-       e_scalar maxChidot = m_chidot.cwise().abs().maxCoeff();
+       e_scalar maxChidot = m_chidot.array().abs().maxCoeff();
        if (timestep*maxChidot > m_maxDeltaChi) {
                timestep = m_maxDeltaChi/maxChidot;
        }
@@ -162,9 +162,9 @@ bool ConstraintSet::closeLoop(){
         }else
             m_B.row(i) = m_U.col(i)/m_S(i);
 
-    m_Jf_inv=(m_V*m_B).lazy();
+    m_Jf_inv.noalias()=m_V*m_B;
 
-    m_chi+=(m_Jf_inv*m_tdelta).lazy();
+    m_chi.noalias()+=m_Jf_inv*m_tdelta;
     updateJacobian();
        // m_externalPose-m_internalPose in end effector frame
        // this is just to compare the pose, a different formula would work too
index 773370b..4055aad 100644 (file)
@@ -54,7 +54,7 @@ const e_matrix& ControlledObject::getJq(unsigned int ee) const
 
 double ControlledObject::getMaxTimestep(double& timestep)
 {
-       e_scalar maxQdot = m_qdot.cwise().abs().maxCoeff();
+       e_scalar maxQdot = m_qdot.array().abs().maxCoeff();
        if (timestep*maxQdot > m_maxDeltaQ) {
                timestep = m_maxDeltaQ/maxQdot;
        }
index 133d6de..a9d4c2f 100644 (file)
@@ -473,7 +473,7 @@ double CopyPose::getMaxTimestep(double& timestep)
        // CopyPose should not have any limit on linear velocity: 
        // in case the target is out of reach, this can be very high.
        // We will simply limit on rotation
-       e_scalar maxChidot = m_chidot.block(3,0,3,1).cwise().abs().maxCoeff();
+       e_scalar maxChidot = m_chidot.block(3,0,3,1).array().abs().maxCoeff();
        if (timestep*maxChidot > m_maxDeltaChi) {
                timestep = m_maxDeltaChi/maxChidot;
        }
index 9b8c5e5..16f8551 100644 (file)
@@ -356,7 +356,7 @@ bool Scene::update(double timestamp, double timestep, unsigned int numsubstep, b
                                        m_Uf.col(i).setConstant(0.0);
                                else
                                        m_Uf.col(i)*=(1/m_Sf(i));
-                       project(m_Jf_inv,cs->featurerange,cs->featurerange)=(m_Vf*m_Uf.transpose()).lazy();
+                       project(m_Jf_inv,cs->featurerange,cs->featurerange).noalias()=m_Vf*m_Uf.transpose();
 
                        //Get the robotjacobian associated with this constraintset
                        //Each jacobian is expressed in robot base frame => convert to world reference
@@ -410,11 +410,11 @@ bool Scene::update(double timestamp, double timestep, unsigned int numsubstep, b
                }
 
            //Calculate A
-               m_Atemp=(m_Cf*m_Jf_inv).lazy();
-               m_A = m_Cq-(m_Atemp*m_Jq).lazy();
+               m_Atemp.noalias()=m_Cf*m_Jf_inv;
+               m_A.noalias() = m_Cq-(m_Atemp*m_Jq);
                if (m_nuTotal > 0) {
-                       m_B=(m_Atemp*m_Ju).lazy();
-                       m_ydot += (m_B*m_xdot).lazy();
+                       m_B.noalias()=m_Atemp*m_Ju;
+                       m_ydot.noalias() += m_B*m_xdot;
                }
 
                //Call the solver with A, Wq, Wy, ydot to solver qdot:
@@ -435,13 +435,13 @@ bool Scene::update(double timestamp, double timestep, unsigned int numsubstep, b
                        //Calculate the twist of the world reference frame due to the robots (Jq*qdot+Ju*chiudot):
                        e_vector6 external_vel = e_zero_vector(6);
                        if (ob1->jointrange.count > 0)
-                               external_vel += (project(m_Jq,cs->featurerange,ob1->jointrange)*project(m_qdot,ob1->jointrange)).lazy();
+                               external_vel.noalias() += (project(m_Jq,cs->featurerange,ob1->jointrange)*project(m_qdot,ob1->jointrange));
                        if (ob2->jointrange.count > 0)
-                               external_vel += (project(m_Jq,cs->featurerange,ob2->jointrange)*project(m_qdot,ob2->jointrange)).lazy();
+                               external_vel.noalias() += (project(m_Jq,cs->featurerange,ob2->jointrange)*project(m_qdot,ob2->jointrange));
                        if (ob1->coordinaterange.count > 0)
-                               external_vel += (project(m_Ju,cs->featurerange,ob1->coordinaterange)*project(m_xdot,ob1->coordinaterange)).lazy();
+                               external_vel.noalias() += (project(m_Ju,cs->featurerange,ob1->coordinaterange)*project(m_xdot,ob1->coordinaterange));
                        if (ob2->coordinaterange.count > 0)
-                               external_vel += (project(m_Ju,cs->featurerange,ob2->coordinaterange)*project(m_xdot,ob2->coordinaterange)).lazy();
+                               external_vel.noalias() += (project(m_Ju,cs->featurerange,ob2->coordinaterange)*project(m_xdot,ob2->coordinaterange));
                        //the twist caused by the constraint must be opposite because of the closed loop
                        //estimate the velocity of the joints using the inverse jacobian
                        e_vector6 estimated_chidot = project(m_Jf_inv,cs->featurerange,cs->featurerange)*(-external_vel);
index a414614..d03c383 100644 (file)
@@ -65,10 +65,10 @@ bool WDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& yd
     if(ret<0)
         return false;
 
-    m_WqV = (Wq*m_V).lazy();
+    m_WqV.noalias() = Wq*m_V;
 
     //Wy*ydot
-       m_Wy_ydot = Wy.cwise() * ydot;
+       m_Wy_ydot = Wy.array() * ydot.array();
     //S^-1*U'*Wy*ydot
        e_scalar maxDeltaS = e_scalar(0.0);
        e_scalar prevS = e_scalar(0.0);
@@ -85,7 +85,7 @@ bool WDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& yd
                }
                lambda = (S < m_epsilon) ? (e_scalar(1.0)-KDL::sqr(S/m_epsilon))*m_lambda*m_lambda : e_scalar(0.0);
                alpha = m_U.col(i).dot(m_Wy_ydot)*S/(S*S+lambda);
-               vmax = m_WqV.col(i).cwise().abs().maxCoeff();
+               vmax = m_WqV.col(i).array().abs().maxCoeff();
                norm = fabs(alpha*vmax);
                if (norm > m_qmax) {
                        qdot += m_WqV.col(i)*(alpha*m_qmax/norm);
index 014c312..6fdd8e6 100644 (file)
@@ -60,7 +60,7 @@ bool WSDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& y
        e_scalar N, M;
 
        // Create the Weighted jacobian
-    m_AWq = (A*Wq).lazy();
+    m_AWq.noalias() = A*Wq;
        for (i=0; i<m_nc; i++)
                m_WyAWq.row(i) = Wy(i)*m_AWq.row(i);
 
@@ -75,8 +75,8 @@ bool WSDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& y
     if(ret<0)
         return false;
 
-       m_Wy_ydot = Wy.cwise() * ydot;
-    m_WqV = (Wq*m_V).lazy();
+       m_Wy_ydot = Wy.array() * ydot.array();
+    m_WqV.noalias() = Wq*m_V;
        qdot.setZero();
        e_scalar maxDeltaS = e_scalar(0.0);
        e_scalar prevS = e_scalar(0.0);
@@ -121,7 +121,7 @@ bool WSDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& y
                M *= Sinv;
                alpha = m_U.col(i).dot(m_Wy_ydot);
                _qmax = (N < M) ? m_qmax*N/M : m_qmax;
-               vmax = m_WqV.col(i).cwise().abs().maxCoeff();
+               vmax = m_WqV.col(i).array().abs().maxCoeff();
                norm = fabs(Sinv*alpha*vmax);
                if (norm > _qmax) {
                        damp = Sinv*alpha*_qmax/norm;