Skip to content

Commit

Permalink
Merge branch 'master' of https://github.com/mposa/drake into initial_…
Browse files Browse the repository at this point in the history 
…conditions
  • Loading branch information
@RussTedrake
RussTedrake committed Sep 26, 2015
2 parents 0774d24 + 73348de commit 108ff7c
Show file tree
Hide file tree
Showing 4 changed files with 159 additions and 83 deletions.
26 changes: 26 additions & 0 deletions drake/systems/plants/RigidBodyManipulator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -493,6 +493,32 @@ class DLLEXPORT_RBM RigidBodyManipulator
template <typename Derived>
Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Eigen::Dynamic> transformVelocityMappingToPositionDotMapping(
const KinematicsCache<typename Derived::Scalar>& cache, const Eigen::MatrixBase<Derived>& mat) const;

/*
template <typename Derived>
Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Eigen::Dynamic> transformPositionDotMappingToVelocityMapping(
const KinematicsCache<typename Derived::Scalar>& cache, const Eigen::MatrixBase<Derived>& mat) const;
*/

template <typename Derived>
Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Eigen::Dynamic> transformPositionDotMappingToVelocityMapping(
const KinematicsCache<typename Derived::Scalar>& cache, const Eigen::MatrixBase<Derived>& mat) const
{
Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Eigen::Dynamic> ret(mat.rows(), num_velocities);
int ret_col_start = 0;
int mat_col_start = 0;
for (auto it = bodies.begin(); it != bodies.end(); ++it) {
RigidBody& body = **it;
if (body.hasParent()) {
const DrakeJoint& joint = body.getJoint();
const auto& element = cache.getElement(body);
ret.middleCols(ret_col_start, joint.getNumVelocities()).noalias() = mat.middleCols(mat_col_start, joint.getNumPositions()) * element.v_to_qdot.value();
ret_col_start += joint.getNumVelocities();
mat_col_start += joint.getNumPositions();
}
}
return ret;
};

template <typename Derived>
Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Eigen::Dynamic> compactToFull(
Expand Down
122 changes: 70 additions & 52 deletions drake/systems/plants/TimeSteppingRigidBodyManipulator.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -190,28 +190,43 @@ function checkDirty(obj)

function [xdn,df] = update(obj,t,x,u)
if (nargout>1)
[obj,z,Mqdn,wqdn,dz,dMqdn,dwqdn] = solveLCP(obj,t,x,u);
[obj,z,Mvn,wvn,dz,dMvn,dwvn] = solveLCP(obj,t,x,u);
else
[obj,z,Mqdn,wqdn] = solveLCP(obj,t,x,u);
[obj,z,Mvn,wvn] = solveLCP(obj,t,x,u);
end

num_q = obj.manip.num_positions;
q=x(1:num_q); qd=x((num_q+1):end);
q=x(1:num_q); v=x((num_q+1):end);
h = obj.timestep;

if isempty(z)
qdn = wqdn;
vn = wvn;
else
qdn = Mqdn*z + wqdn;
vn = Mvn*z + wvn;
[obj2,z2,Mvn2,wvn2] = solveMexLCP(obj,t,x,u);
end

vToqdot = obj.manip.vToqdot(q);
qdn = vToqdot*vn;
qn = q+ h*qdn;
% Find quaternion indices
quat_bodies = obj.manip.body([obj.manip.body.floating] == 2);
quat_positions = [quat_bodies.position_num];
for i=1:size(quat_positions,2)
quat_dot = qdn(quat_positions(4:7,i));
if norm(quat_dot) > 0
% Update quaternion by following geodesic
qn(quat_positions(4:7,i)) = q(quat_positions(4:7,i)) + quat_dot/norm(quat_dot)*tan(norm(h*quat_dot));
qn(quat_positions(4:7,i)) = qn(quat_positions(4:7,i))/norm(qn(quat_positions(4:7,i)));
end
end
qn = q + h*qdn;
xdn = [qn;qdn];
xdn = [qn;vn];

if (nargout>1) % compute gradients
if isempty(z)
dqdn = dwqdn;
dqdn = dwvn;
else
dqdn = matGradMult(dMqdn,z) + Mqdn*dz + dwqdn;
dqdn = matGradMult(dMvn,z) + Mvn*dz + dwvn;
end
df = [ [zeros(num_q,1), eye(num_q), zeros(num_q,num_q+obj.num_u)]+h*dqdn; dqdn ];
end
Expand Down Expand Up @@ -260,9 +275,9 @@ function checkDirty(obj)
obj.LCP_cache.data.possible_limit_indices = logical(possible_jointlimit_indices);
end

function [obj,z,Mqdn,wqdn,dz,dMqdn,dwqdn] = solveLCP(obj,t,x,u)
function [obj,z,Mvn,wvn,dz,dMvn,dwvn] = solveLCP(obj,t,x,u)
if (nargout<5 && obj.gurobi_present && obj.manip.only_loops && obj.manip.mex_model_ptr~=0 && ~obj.position_control)
[obj,z,Mqdn,wqdn] = solveMexLCP(obj,t,x,u);
[obj,z,Mvn,wvn] = solveMexLCP(obj,t,x,u);
return;
end

Expand All @@ -271,12 +286,12 @@ function checkDirty(obj)
% todo: implement some basic caching here
if cacheHit(obj,t,x,u,nargout)
z = obj.LCP_cache.data.z;
Mqdn = obj.LCP_cache.data.Mqdn;
wqdn = obj.LCP_cache.data.wqdn;
Mvn = obj.LCP_cache.data.Mqdn;
wvn = obj.LCP_cache.data.wqdn;
if nargout > 4
dz = obj.LCP_cache.data.dz;
dMqdn = obj.LCP_cache.data.dMqdn;
dwqdn = obj.LCP_cache.data.dwqdn;
dMvn = obj.LCP_cache.data.dMqdn;
dwvn = obj.LCP_cache.data.dwqdn;
end
else

Expand All @@ -285,27 +300,30 @@ function checkDirty(obj)
obj.LCP_cache.data.u = u;
obj.LCP_cache.data.nargout = nargout;

num_q = obj.manip.num_positions;
q=x(1:num_q); qd=x(num_q+(1:num_q));
num_q = obj.manip.getNumPositions;
num_v = obj.manip.getNumVelocities;
q=x(1:num_q); v=x(num_q+(1:num_v));
vToqdot = obj.manip.vToqdot(q);
qd = vToqdot*v;
h = obj.timestep;

kinsol = doKinematics(obj,q);

if (nargout<5)
[H,C,B] = manipulatorDynamics(obj.manip,q,qd);
[H,C,B] = manipulatorDynamics(obj.manip,q,v);
if (obj.num_u>0 && ~obj.position_control)
tau = B*u - C;
else
tau = -C;
end
else
[H,C,B,dH,dC,dB] = manipulatorDynamics(obj.manip,q,qd);
[H,C,B,dH,dC,dB] = manipulatorDynamics(obj.manip,q,v);
if (obj.num_u>0 && ~obj.position_control)
tau = B*u - C;
dtau = [zeros(num_q,1), matGradMult(dB,u) - dC, B];
dtau = [zeros(num_v,1), matGradMult(dB,u) - dC, B];
else
tau = -C;
dtau = [zeros(num_q,1), -dC, zeros(size(B))];
dtau = [zeros(num_v,1), -dC, zeros(size(B))];
end
end

Expand Down Expand Up @@ -461,8 +479,8 @@ function checkDirty(obj)

if (nC+nL+nP+nV==0) % if there are no possible contacts
z = [];
Mqdn = [];
wqdn = qd + h*(H\tau);
Mvn = [];
wvn = v + h*(H\tau);
if (nargout>4)
error('need to implement this case');
end
Expand Down Expand Up @@ -497,16 +515,16 @@ function checkDirty(obj)
w = zeros(nL+nP+(mC+2)*nC,1)*q(1);

Hinv = inv(H);
wqdn = qd + h*Hinv*tau;
Mqdn = Hinv*J';
wvn = v + h*Hinv*tau;
Mvn = Hinv*vToqdot'*J';

if (nargout>4)
dM = zeros(size(M,1),size(M,2),1+2*num_q+obj.num_u);
dw = zeros(size(w,1),1+2*num_q+obj.num_u);
dwqdn = [zeros(num_q,1+num_q),eye(num_q),zeros(num_q,obj.num_u)] + ...
h*Hinv*dtau - [zeros(num_q,1),h*Hinv*matGradMult(dH(:,1:num_q),Hinv*tau),zeros(num_q,num_q),zeros(num_q,obj.num_u)];
dM = zeros(size(M,1),size(M,2),1+num_q+num_v+obj.num_u);
dw = zeros(size(w,1),1+num_q+num_v+obj.num_u);
dwvn = [zeros(num_v,1+num_q),eye(num_v),zeros(num_v,obj.num_u)] + ...
h*Hinv*dtau - [zeros(num_v,1),h*Hinv*matGradMult(dH(:,1:num_q),Hinv*tau),zeros(num_v,num_q),zeros(num_v,obj.num_u)];
dJtranspose = reshape(permute(reshape(dJ,size(J,1),size(J,2),[]),[2,1,3]),numel(J),[]);
dMqdn = [zeros(numel(Mqdn),1),reshape(Hinv*reshape(dJtranspose - matGradMult(dH(:,1:num_q),Hinv*J'),num_q,[]),numel(Mqdn),[]),zeros(numel(Mqdn),num_q+obj.num_u)];
dMvn = [zeros(numel(Mvn),1),reshape(Hinv*reshape(dJtranspose - matGradMult(dH(:,1:num_q),Hinv*J'),num_q,[]),numel(Mvn),[]),zeros(numel(Mvn),num_v+obj.num_u)];
end

% check gradients
Expand All @@ -523,29 +541,29 @@ function checkDirty(obj)
% z(1:nL) = cL (nL includes min AND max; 0<=nL<=2*num_q)
% s(1:nL) = phiL(qn) approx phiL + h*JL*qdn
if (nL > 0)
w(1:nL) = phiL + h*JL*wqdn;
M(1:nL,:) = h*JL*Mqdn;
w(1:nL) = phiL + h*JL*wvn;
M(1:nL,:) = h*JL*Mvn;
if (nargout>4)
dJL = [zeros(numel(JL),1),reshape(dJL,numel(JL),[]),zeros(numel(JL),num_q+obj.num_u)];
if (obj.position_control)
dw(1:nL,:) = [zeros(size(JL,1),1),JL,zeros(size(JL,1),num_q),...
[-1*ones(length(pos_control_index),obj.num_u);1*ones(length(pos_control_index),obj.num_u)]] + h*matGradMultMat(JL,wqdn,dJL,dwqdn);
[-1*ones(length(pos_control_index),obj.num_u);1*ones(length(pos_control_index),obj.num_u)]] + h*matGradMultMat(JL,wvn,dJL,dwvn);
else
dw(1:nL,:) = [zeros(size(JL,1),1),JL,zeros(size(JL,1),num_q+obj.num_u)] + h*matGradMultMat(JL,wqdn,dJL,dwqdn);
dw(1:nL,:) = [zeros(size(JL,1),1),JL,zeros(size(JL,1),num_q+obj.num_u)] + h*matGradMultMat(JL,wvn,dJL,dwvn);
end
dM(1:nL,1:size(Mqdn,2),:) = reshape(h*matGradMultMat(JL,Mqdn,dJL,dMqdn),nL,size(Mqdn,2),[]);
dM(1:nL,1:size(Mvn,2),:) = reshape(h*matGradMultMat(JL,Mvn,dJL,dMvn),nL,size(Mvn,2),[]);
end
end

%% Bilateral Position Constraints:
% enforcing eq7, line 2
if (nP > 0)
w(nL+(1:nP)) = phiP + h*JP*wqdn;
M(nL+(1:nP),:) = h*JP*Mqdn;
w(nL+(1:nP)) = phiP + h*JP*wvn;
M(nL+(1:nP),:) = h*JP*Mvn;
if (nargout>4)
dJP = [zeros(numel(JP),1),reshape(dJP,numel(JP),[]),zeros(numel(JP),num_q+obj.num_u)];
dw(nL+(1:nP),:) = [zeros(size(JP,1),1),JP,zeros(size(JP,1),num_q+obj.num_u)] + h*matGradMultMat(JP,wqdn,dJP,dwqdn);
dM(nL+(1:nP),1:size(Mqdn,2),:) = reshape(h*matGradMultMat(JP,Mqdn,dJP,qMqdn),nP,size(Mqdn,2),[]);
dw(nL+(1:nP),:) = [zeros(size(JP,1),1),JP,zeros(size(JP,1),num_q+obj.num_u)] + h*matGradMultMat(JP,wvn,dJP,dwvn);
dM(nL+(1:nP),1:size(Mvn,2),:) = reshape(h*matGradMultMat(JP,Mvn,dJP,qMqdn),nP,size(Mvn,2),[]);
end
end

Expand Down Expand Up @@ -587,11 +605,11 @@ function checkDirty(obj)
% of these two can exist. (the first is actually many solutions, with
% beta_i in opposite directions canceling each other out).
if (nC > 0)
w(nL+nP+(1:nC)) = phiC+h*n*wqdn;
M(nL+nP+(1:nC),:) = h*n*Mqdn;
w(nL+nP+(1:nC)) = phiC+h*n*vToqdot*wvn;
M(nL+nP+(1:nC),:) = h*n*vToqdot*Mvn;

w(nL+nP+nC+(1:mC*nC)) = D*wqdn;
M(nL+nP+nC+(1:mC*nC),:) = D*Mqdn;
w(nL+nP+nC+(1:mC*nC)) = D*vToqdot*wvn;
M(nL+nP+nC+(1:mC*nC),:) = D*vToqdot*Mvn;
M(nL+nP+nC+(1:mC*nC),nL+nP+(1+mC)*nC+(1:nC)) = repmat(eye(nC),mC,1);

M(nL+nP+(mC+1)*nC+(1:nC),nL+nP+(1:(mC+1)*nC)) = [diag(mu), repmat(-eye(nC),1,mC)];
Expand All @@ -601,11 +619,11 @@ function checkDirty(obj)
dn = [zeros(size(dn,1),1),dn,zeros(size(dn,1),num_q+obj.num_u)];
dD = [zeros(numel(D),1),reshape(dD,numel(D),[]),zeros(numel(D),num_q+obj.num_u)];

dw(nL+nP+(1:nC),:) = [zeros(size(n,1),1),n,zeros(size(n,1),num_q+obj.num_u)]+h*matGradMultMat(n,wqdn,dn,dwqdn);
dM(nL+nP+(1:nC),1:size(Mqdn,2),:) = reshape(h*matGradMultMat(n,Mqdn,dn,dMqdn),nC,size(Mqdn,2),[]);
dw(nL+nP+(1:nC),:) = [zeros(size(n,1),1),n,zeros(size(n,1),num_q+obj.num_u)]+h*matGradMultMat(n,wvn,dn,dwvn);
dM(nL+nP+(1:nC),1:size(Mvn,2),:) = reshape(h*matGradMultMat(n,Mvn,dn,dMvn),nC,size(Mvn,2),[]);

dw(nL+nP+nC+(1:mC*nC),:) = matGradMultMat(D,wqdn,dD,dwqdn);
dM(nL+nP+nC+(1:mC*nC),1:size(Mqdn,2),:) = reshape(matGradMultMat(D,Mqdn,dD,dMqdn),mC*nC,size(Mqdn,2),[]);
dw(nL+nP+nC+(1:mC*nC),:) = matGradMultMat(D,wvn,dD,dwvn);
dM(nL+nP+nC+(1:mC*nC),1:size(Mvn,2),:) = reshape(matGradMultMat(D,Mvn,dD,dMvn),mC*nC,size(Mvn,2),[]);
end
end

Expand Down Expand Up @@ -691,8 +709,8 @@ function checkDirty(obj)
end

obj.LCP_cache.data.z = z;
obj.LCP_cache.data.Mqdn = Mqdn;
obj.LCP_cache.data.wqdn = wqdn;
obj.LCP_cache.data.Mqdn = Mvn;
obj.LCP_cache.data.wqdn = wvn;

if (nargout>4)
% Quick derivation:
Expand Down Expand Up @@ -729,15 +747,15 @@ function checkDirty(obj)
dz(zposind,:) = -pinv(Mbar)*(matGradMult(dMbar,zbar) + dwbar);
end
obj.LCP_cache.data.dz = dz;
obj.LCP_cache.data.dMqdn = dMqdn;
obj.LCP_cache.data.dwqdn = dwqdn;
obj.LCP_cache.data.dMqdn = dMvn;
obj.LCP_cache.data.dwqdn = dwvn;
else
obj.LCP_cache.data.dz = [];
obj.LCP_cache.data.dMqdn = [];
obj.LCP_cache.data.dwqdn = [];
end

penetration = phi_check + h*J_check*(Mqdn*z + wqdn) < 0;
penetration = phi_check + h*J_check*vToqdot*(Mvn*z + wvn) < 0;
if any(penetration)
% then add the constraint and run the entire loop again!
limits = sum(~possible_limit_indices);
Expand Down
Loading

0 comments on commit 108ff7c

Please sign in to comment.