From 8a6a3dbb547bc87759fda875ad2fdd85bb32835c Mon Sep 17 00:00:00 2001 From: Bastien Montagne Date: Sun, 23 Oct 2011 19:39:20 +0000 Subject: Fix for commit 41227 (Some opening comment tags (/*) were lost!). --- intern/itasc/ConstraintSet.cpp | 6 +++--- intern/itasc/ControlledObject.cpp | 2 +- intern/itasc/CopyPose.cpp | 2 +- intern/itasc/Scene.cpp | 18 +++++++++--------- intern/itasc/WDLSSolver.cpp | 6 +++--- intern/itasc/WSDLSSolver.cpp | 8 ++++---- 6 files changed, 21 insertions(+), 21 deletions(-) (limited to 'intern') diff --git a/intern/itasc/ConstraintSet.cpp b/intern/itasc/ConstraintSet.cpp index e21cd36e4c8..74926df3092 100644 --- a/intern/itasc/ConstraintSet.cpp +++ b/intern/itasc/ConstraintSet.cpp @@ -80,7 +80,7 @@ void ConstraintSet::modelUpdate(Frame& _external_pose,const Timestamp& timestamp double ConstraintSet::getMaxTimestep(double& timestep) { - e_scalar maxChidot = m_chidot.array().abs().maxCoeff(); + e_scalar maxChidot = m_chidot.cwise().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.noalias()=m_V*m_B; + m_Jf_inv=(m_V*m_B).lazy(); - m_chi.noalias()+=m_Jf_inv*m_tdelta; + m_chi+=(m_Jf_inv*m_tdelta).lazy(); updateJacobian(); // m_externalPose-m_internalPose in end effector frame // this is just to compare the pose, a different formula would work too diff --git a/intern/itasc/ControlledObject.cpp b/intern/itasc/ControlledObject.cpp index 4055aad7768..773370b6d2e 100644 --- a/intern/itasc/ControlledObject.cpp +++ b/intern/itasc/ControlledObject.cpp @@ -54,7 +54,7 @@ const e_matrix& ControlledObject::getJq(unsigned int ee) const double ControlledObject::getMaxTimestep(double& timestep) { - e_scalar maxQdot = m_qdot.array().abs().maxCoeff(); + e_scalar maxQdot = m_qdot.cwise().abs().maxCoeff(); if (timestep*maxQdot > m_maxDeltaQ) { timestep = m_maxDeltaQ/maxQdot; } diff --git a/intern/itasc/CopyPose.cpp b/intern/itasc/CopyPose.cpp index a9d4c2f0a2a..133d6de2069 100644 --- a/intern/itasc/CopyPose.cpp +++ b/intern/itasc/CopyPose.cpp @@ -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).array().abs().maxCoeff(); + e_scalar maxChidot = m_chidot.block(3,0,3,1).cwise().abs().maxCoeff(); if (timestep*maxChidot > m_maxDeltaChi) { timestep = m_maxDeltaChi/maxChidot; } diff --git a/intern/itasc/Scene.cpp b/intern/itasc/Scene.cpp index 16f8551bfc7..9b8c5e5cd4c 100644 --- a/intern/itasc/Scene.cpp +++ b/intern/itasc/Scene.cpp @@ -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).noalias()=m_Vf*m_Uf.transpose(); + project(m_Jf_inv,cs->featurerange,cs->featurerange)=(m_Vf*m_Uf.transpose()).lazy(); //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.noalias()=m_Cf*m_Jf_inv; - m_A.noalias() = m_Cq-(m_Atemp*m_Jq); + m_Atemp=(m_Cf*m_Jf_inv).lazy(); + m_A = m_Cq-(m_Atemp*m_Jq).lazy(); if (m_nuTotal > 0) { - m_B.noalias()=m_Atemp*m_Ju; - m_ydot.noalias() += m_B*m_xdot; + m_B=(m_Atemp*m_Ju).lazy(); + m_ydot += (m_B*m_xdot).lazy(); } //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.noalias() += (project(m_Jq,cs->featurerange,ob1->jointrange)*project(m_qdot,ob1->jointrange)); + external_vel += (project(m_Jq,cs->featurerange,ob1->jointrange)*project(m_qdot,ob1->jointrange)).lazy(); if (ob2->jointrange.count > 0) - external_vel.noalias() += (project(m_Jq,cs->featurerange,ob2->jointrange)*project(m_qdot,ob2->jointrange)); + external_vel += (project(m_Jq,cs->featurerange,ob2->jointrange)*project(m_qdot,ob2->jointrange)).lazy(); if (ob1->coordinaterange.count > 0) - external_vel.noalias() += (project(m_Ju,cs->featurerange,ob1->coordinaterange)*project(m_xdot,ob1->coordinaterange)); + external_vel += (project(m_Ju,cs->featurerange,ob1->coordinaterange)*project(m_xdot,ob1->coordinaterange)).lazy(); if (ob2->coordinaterange.count > 0) - external_vel.noalias() += (project(m_Ju,cs->featurerange,ob2->coordinaterange)*project(m_xdot,ob2->coordinaterange)); + external_vel += (project(m_Ju,cs->featurerange,ob2->coordinaterange)*project(m_xdot,ob2->coordinaterange)).lazy(); //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); diff --git a/intern/itasc/WDLSSolver.cpp b/intern/itasc/WDLSSolver.cpp index d03c38396c7..a4146149d30 100644 --- a/intern/itasc/WDLSSolver.cpp +++ b/intern/itasc/WDLSSolver.cpp @@ -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.noalias() = Wq*m_V; + m_WqV = (Wq*m_V).lazy(); //Wy*ydot - m_Wy_ydot = Wy.array() * ydot.array(); + m_Wy_ydot = Wy.cwise() * ydot; //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).array().abs().maxCoeff(); + vmax = m_WqV.col(i).cwise().abs().maxCoeff(); norm = fabs(alpha*vmax); if (norm > m_qmax) { qdot += m_WqV.col(i)*(alpha*m_qmax/norm); diff --git a/intern/itasc/WSDLSSolver.cpp b/intern/itasc/WSDLSSolver.cpp index 6fdd8e6e06c..014c312635f 100644 --- a/intern/itasc/WSDLSSolver.cpp +++ b/intern/itasc/WSDLSSolver.cpp @@ -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.noalias() = A*Wq; + m_AWq = (A*Wq).lazy(); for (i=0; i _qmax) { damp = Sinv*alpha*_qmax/norm; -- cgit v1.2.3