Skip to content
Snippets Groups Projects
Commit c49cdf5e authored by Bastien Montagne's avatar Bastien Montagne
Browse files

Another set of UI messages fixes and tweaks! No functional changes.

parent 8a6a3dbb
Branches
Tags
No related merge requests found
# -*- 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 #####
#
......
#!/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
......
......@@ -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
......
......@@ -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;
}
......
......@@ -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;
}
......
......@@ -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);
......
......@@ -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);
......
......@@ -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;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment