From c49cdf5eec1ec70008c12f7b6cbfdad75dde3b16 Mon Sep 17 00:00:00 2001 From: Bastien Montagne Date: Sun, 23 Oct 2011 19:54:06 +0000 Subject: Another set of UI messages fixes and tweaks! No functional changes. --- 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/itasc') diff --git a/intern/itasc/ConstraintSet.cpp b/intern/itasc/ConstraintSet.cpp index 74926df3092..e21cd36e4c8 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.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 diff --git a/intern/itasc/ControlledObject.cpp b/intern/itasc/ControlledObject.cpp index 773370b6d2e..4055aad7768 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.cwise().abs().maxCoeff(); + e_scalar maxQdot = m_qdot.array().abs().maxCoeff(); if (timestep*maxQdot > m_maxDeltaQ) { timestep = m_maxDeltaQ/maxQdot; } diff --git a/intern/itasc/CopyPose.cpp b/intern/itasc/CopyPose.cpp index 133d6de2069..a9d4c2f0a2a 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).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; } diff --git a/intern/itasc/Scene.cpp b/intern/itasc/Scene.cpp index 9b8c5e5cd4c..16f8551bfc7 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)=(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); diff --git a/intern/itasc/WDLSSolver.cpp b/intern/itasc/WDLSSolver.cpp index a4146149d30..d03c38396c7 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 = (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); diff --git a/intern/itasc/WSDLSSolver.cpp b/intern/itasc/WSDLSSolver.cpp index 014c312635f..6fdd8e6e06c 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 = (A*Wq).lazy(); + m_AWq.noalias() = A*Wq; for (i=0; i _qmax) { damp = Sinv*alpha*_qmax/norm; -- cgit v1.2.3