function [q0,qset] = testlaw_PP(q_target,qd_target,qdd_target,Time)


%On the second and fourth joint is necessary to put a "+pi" to take into
%account the offset caused by the simscape dynamic model.
q0 = q_target(1,:);
% q0(2) = q0(2)+pi; 
% q0(4) = q0(4)+pi;


for i = 1:6
    pos.signals.values(:,1,i) = q_target(:,i);
    pos.signals.values(:,2,i) = qd_target(:,i);
    pos.signals.values(:,3,i) = qdd_target(:,i);
end

time = linspace(0,Time,length(q_target));
qset.q1.time = time;
qset.q1.signals.values = pos.signals.values(:,:,1);
qset.q2.time = time;
qset.q2.signals.values = pos.signals.values(:,:,2);
% qset.q2.signals.values(:,1) = qset.q2.signals.values(:,1)+pi;
qset.q2.signals.values(:,1) = qset.q2.signals.values(:,1);
qset.q3.time = time;
qset.q3.signals.values = pos.signals.values(:,:,3);
qset.q4.time = time;
qset.q4.signals.values = pos.signals.values(:,:,4);
% qset.q4.signals.values(:,1) = qset.q4.signals.values(:,1)+pi;
qset.q4.signals.values(:,1) = qset.q4.signals.values(:,1);
qset.q5.time = time;
qset.q5.signals.values = pos.signals.values(:,:,5);
qset.q6.time = time;
qset.q6.signals.values = pos.signals.values(:,:,6);

end
