WorldPositionConstraint seems to be violated in modified runAtlasHandControl example.

[AndreasMegatranus]

I've been experimenting with runAtlasHandControl, and replaced the WorldEulerConstraints used to specify r_hand_cnstr with a new set. The new set is:

goal_position_constraint = WorldPositionConstraint(r,r_hand,zeros(3,1),[0.2832; -0.4185; 0.5360], ... [0.2832; -0.4185; 0.5460],[1,1]);

operation_position_constraint = WorldPositionConstraint(r,r_hand,zeros(3,1),[0.2; -0.44; 0.51], ... [0.36; -0.38; 0.55],[0,1]);

operation_euler_constraint = WorldEulerConstraint(r,r_hand,[1.0;0.0;1.8],[1.4;0.4;2.2],[0,1]);

r_hand_cnstr = {goal_position_constraint, operation_euler_constraint, operation_position_constraint};

After the trajectory is generated (inverseKinTraj), I check the values by doing

n_breaks = 20; t_breaks = linspace(t_plan(1),t_plan(end),n_breaks);

x_breaks = xtraj.eval(t_breaks);

kinsol_x_breaks_1 = r.doKinematics(x_breaks(1:36,1), x_breaks(37:72,1)); r_hand_pos_x_breaks_1 = r.forwardKin(kinsol_x_breaks_1,r_hand,r_hand_pt,1);

kinsol_x_breaks_10 = r.doKinematics(x_breaks(1:36,10), x_breaks(37:72,10)); r_hand_pos_x_breaks_10 = r.forwardKin(kinsol_x_breaks_10,r_hand,r_hand_pt,1);

The value of r_hand_pos_x_breaks_1 (initial value) is

0.2832

-0.4185 0.5260 1.2256 0.2351 1.9919

which is consistent with the constraints.

The value of r_hand_pos_x_breaks_10 is

0.2589

-0.3809 0.3630 1.3449 0.0778 2.0856

which is not consistent (z value violates the lower bound).

Am I doing something wrong here?

[hongkai-dai]

When you call inverseKinTraj, what value do you pass to the t argument, and did you call function IKoptions.addAdditionaltSamples? inverseKinTraj constructs a cubic spline, it uses the argument t as the knot points for the cubic spline, and it guarantees that the constraints are satisfied at t and the additional t samples (the argument passed in from IKoptions.addAdditionaltSamples). So if you want your constraints to be satisfied at t_breaks = linspace(t_plan(1),t_plan(end),n_breaks);, you can call

options = IKoptions(r);
options = options.addAdditionaltSamples(t_breaks);
[xtraj,info] = inverseKinTraj(r,t,q_seed_traj,q_nom_traj,CONSTRAINT,options)

Let me know if you still find the constraints being violated

[AndreasMegatranus]

I passed in more points in the t argument, but still found that the
constraints (constraints for z direction) are violated at exactly
those points. I've attached the code. It is the original
runAtlasHandControl, with edits to the constraints and call to
inverseKinTraj.

Is the initial pose near some sort of singularity or infeasibility?
I'm just trying to move the hand up a bit (in the z direction) from its initial position.

Andreas

Quoting Hongkai Dai notifications@github.com:

When you call inverseKinTraj, what value do you pass to the t
argument, and did you call function
IKoptions.addAdditionaltSamples? inverseKinTraj constructs a
cubic spline, it uses the argument t as the knot points for the
cubic spline, and it guarantees that the constraints are satisfied
at t and the additional t samples (the argument passed in from
IKoptions.addAdditionaltSamples). So if you want your constraints to
be satisfied at t_breaks = linspace(t_plan(1),t_plan(end),n_breaks);, you can call

options = IKoptions(r);
options = options.addAdditionaltSamples(t_breaks);
[xtraj,info] = inverseKinTraj(r,t,q_seed_traj,q_nom_traj,CONSTRAINT,options)

Let me know if you still find the constraints being violated

---

You are receiving this because you authored the thread. Reply to this email directly or view it on GitHub: https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-206897232

function runAtlasHandControl(example_options)

% Tests out the piping to add a robotiq hand (with kinematic loops) % to Atlas, and hook up a controller to command its joints

if ~checkDependency('gurobi') warning('Must have gurobi installed to run this example'); return; end

import bipedControllers.*

path_handle = addpathTemporary(fullfile(getDrakePath(), 'examples', 'ZMP'));

% put robot in a random x,y,yaw position and balance for 2 seconds if nargin<1, example_options=struct(); end example_options = applyDefaults(example_options, struct('use_mex', true, ... 'use_bullet', false,... 'visualize', true,... 'terrain', RigidBodyFlatTerrain));

if (nargin<1); use_mex = true; end if (nargin<2); use_angular_momentum = false; end

% silence some warnings warning('off','Drake:RigidBodyManipulator:UnsupportedContactPoints') warning('off','Drake:RigidBodyManipulator:UnsupportedVelocityLimits')

options.floating = true; options.ignore_self_collisions = true; options.ignore_friction = true; options.dt = 0.001; options.terrain = example_options.terrain; options.use_bullet = example_options.use_bullet; options.use_mex = example_options.use_mex; options.hand_left = 'robotiq_weight_only'; options.hand_right = 'robotiq_weight_only'; r = Atlas('urdf/atlas_minimal_contact.urdf',options); options.hand_left = 'robotiq_tendons'; options.hand_right = 'robotiq_tendons'; r_hands = Atlas('urdf/atlas_minimal_contact.urdf',options); r = r.removeCollisionGroupsExcept({'heel','toe'}); r_hands = r_hands.removeCollisionGroupsExcept({'heel','toe'}); r = compile(r); r_hands = compile(r_hands);

nq = getNumPositions(r); nv = getNumVelocities(r);

keyboard

% set initial state to fixed point load('data/atlas_fp.mat'); xstar(1) = 0; %0.1_randn(); xstar(2) = 0; %0.1_randn(); xstar(6) = 0;% pi*randn(); r = r.setInitialState(xstar);

keyboard

% and hand state to a feasible sol % so lcp has an easier time %load('data/robotiq_feas.mat'); xstar_hands = r_hands.getInitialState(); xstar_hands(1:length(xstar)) = xstar; %xstar_hands = r_hands.getStateFrame().mergeCoordinates({xstar,xstar_hand}); xstar_hands = r_hands.resolveConstraints(xstar_hands); r_hands = r_hands.setInitialState(xstar_hands);

keyboard

x0 = xstar; q0 = x0(1:nq); v0 = x0(nq+(1:nv)); pelvis = r.findLinkId('pelvis'); cnstr = fix_feet_cnstr(r,x0); r_hand = r.findLinkId('r_hand'); r_hand_pt = [0;-0.2;0]; kinsol0 = r.doKinematics(q0,v0); r_hand_pos0 = r.forwardKin(kinsol0,r_hand,r_hand_pt,2);

rpy_r_hand_pos0 = quat2rpy(r_hand_pos0(4:7));

r_hand_pos0_euler = r.forwardKin(kinsol0,r_hand,r_hand_pt,1);

% r_hand_cnstr = {WorldEulerConstraint(r,r_hand,[nan;pi/2;nan],[nan;pi/2;nan],[0.3 0.7]),... % WorldEulerConstraint(r,r_hand,[nan;pi/4;nan],[nan;0.6*pi;nan],[1,1])};

% Include position as well as orientation constraints.

goal_position_constraint = WorldPositionConstraint(r,r_hand,zeros(3,1),[0.2832; -0.4185; 0.5360], ... [0.2832; -0.4185; 0.5460],[1,1]);

goal_euler_constraint = WorldEulerConstraint(r,r_hand,[nan;pi/4;nan],[nan;0.6*pi;nan],[1,1]);

operation_position_constraint = WorldPositionConstraint(r,r_hand,zeros(3,1),[0.2; -0.44; 0.51], ... [0.36; -0.38; 0.55],[0,1]); % Kinematic flow tube.

operation_euler_constraint = WorldEulerConstraint(r,r_hand,[1.0;0.0;1.8],[1.4;0.4;2.2],[0,1]);

r_hand_cnstr = {goal_position_constraint, operation_euler_constraint, operation_position_constraint};

% Really do need orientation constraints!

qtraj0 = PPTrajectory(zoh([0,1],[q0 q0]));

% t_plan = [0,0.5,1];

t_plan = [0:0.05:1]; % Try more knot points.

n_breaks = 20; t_breaks = linspace(t_plan(1),t_plan(end),n_breaks);

t_plan = t_breaks;

[xtraj,info] = inverseKinTraj(r,t_plan,qtraj0,qtraj0,cnstr{:},r_hand_cnstr{:});

keyboard

x_breaks = xtraj.eval(t_breaks);

kinsol_x_breaks_1 = r.doKinematics(x_breaks(1:36,1), x_breaks(37:72,1)); r_hand_pos_x_breaks_1 = r.forwardKin(kinsol_x_breaks_1,r_hand,r_hand_pt,1);

kinsol_x_breaks_2 = r.doKinematics(x_breaks(1:36,2), x_breaks(37:72,2)); r_hand_pos_x_breaks_2 = r.forwardKin(kinsol_x_breaks_2,r_hand,r_hand_pt,1);

kinsol_x_breaks_10 = r.doKinematics(x_breaks(1:36,10), x_breaks(37:72,10)); r_hand_pos_x_breaks_10 = r.forwardKin(kinsol_x_breaks_10,r_hand,r_hand_pt,1);

kinsol_x_breaks_20 = r.doKinematics(x_breaks(1:36,20), x_breaks(37:72,20)); r_hand_pos_x_breaks_20 = r.forwardKin(kinsol_x_breaks_20,r_hand,r_hand_pt,1);

keyboard

t_breaks = t_breaks_4; x_breaks(nq+(1:nv),:) = 1/4_x_breaks(nq+(1:nv),:); qtraj_pp = spline(t_breaks,[zeros(nq,1) x_breaks(1:nq,:) zeros(nq,1)]);

manip_plan_data = QPLocomotionPlanSettings.fromQuasistaticQTraj(r,PPTrajectory(qtraj_pp),... struct('bodies_to_track',[pelvis,r_hand],... 'quat_task_to_world',repmat(uniformlyRandomQuat(),1,2),... 'translation_task_to_world',randn(3,2),... 'is_quasistatic',true,... 'gain_set', 'manip')); r_arm_idx = r.findPositionIndices('r_arm');

manip_plan_data.untracked_joint_inds = r_arm_idx; control = InstantaneousQPController(r, []); planeval = BipedPlanEval(r, QPLocomotionPlanCPPWrapper(manip_plan_data)); plancontroller = BipedPlanEvalAndControlSystem(r, control, planeval);

ins(1).system = 2; ins(1).input = 2; ins(2).system = 2; ins(2).input = 3; outs(1).system = 2; outs(1).output = 1; outs(2).system = 2; outs(2).output = 2; outs(3).system = 2; outs(3).output = 3; sys = mimoFeedback(plancontroller,r_hands,[],[],ins,outs); clear ins; clear outs;

ins(1).system = 1; ins(1).input = 2; outs(1).system = 1; outs(1).output = 1; outs(2).system = 1; outs(2).output = 2; outs(3).system = 1; outs(3).output = 3;

rh = RobotiqControlBlock(rhands, 2, 'right'); sys = mimoFeedback(sys, rh, [], [], ins, outs);

lh = RobotiqControlBlock(rhands, 3, 'left'); sys = mimoFeedback(sys, lh, [], [], [], outs);

if example_options.visualize v = r_hands.constructVisualizer; v.display_dt = 0.01; S=warning('off','Drake:DrakeSystem:UnsupportedSampleTime'); output_select(1).system=1; output_select(1).output=1; output_select(2).system=1; output_select(2).output=2; output_select(3).system = 1; output_select(3).output = 3; sys = mimoCascade(sys,v,[],[],output_select); warning(S); end x0 = xstar_hands; T = min(manip_plan_data.duration + 1, 30); traj = simulate(sys,[0 T],x0); if example_options.visualize % This doesn't see hand movements. Why? playback(v,traj,struct('slider',true)); end

%xf = traj.eval(traj.tspan(2));

function cnstr = fix_feet_cnstr(r,x0) q0 = x0(1:r.getNumPositions); v0 = x0(r.getNumPositions+(1:r.getNumVelocities)); kinsol0 = r.doKinematics(q0,v0,false); l_foot = r.findLinkId('l_foot'); r_foot = r.findLinkId('r_foot'); l_foot_pts = r.getBody(l_foot).getTerrainContactPoints; r_foot_pts = r.getBody(r_foot).getTerrainContactPoints; l_foot_pos0 = r.forwardKin(kinsol0,l_foot,zeros(3,1),2); r_foot_pos0 = r.forwardKin(kinsol0,r_foot,zeros(3,1),2); lfoot_cnstr = {WorldPositionConstraint(r,l_foot,zeros(3,1),l_foot_pos0(1:3),l_foot_pos0(1:3)),... WorldQuatConstraint(r,l_foot,l_foot_pos0(4:7),0)}; rfoot_cnstr = {WorldPositionConstraint(r,r_foot,zeros(3,1),r_foot_pos0(1:3),r_foot_pos0(1:3)),... WorldQuatConstraint(r,r_foot,r_foot_pos0(4:7),0)}; qsc = QuasiStaticConstraint(r,[-inf,inf],1); qsc = qsc.addContact(l_foot,l_foot_pts,r_foot,r_foot_pts); qsc = qsc.setShrinkFactor(0.6); qsc = qsc.setActive(true); utorso = r.findLinkId('utorso'); utorso_upright = WorldGazeDirConstraint(r,utorso,[0;0;1],[0;0;1],0.05*pi); cnstr = [lfoot_cnstr,rfoot_cnstr,{qsc,utorso_upright}]; end

end

[RussTedrake]

what is the return code from snopt?

[hongkai-dai]

I got info 32 when running your code, which means the major iteration limit is reached, and the solver fails to solve the problem. That is why the constraints are violated. You can try

t_plan = linspace(0,1,10);
ikoptions = IKoptions(r);
ikoptions = ikoptions.setMajorIterationsLimit(1e4);
ikoptions = ikoptions.setIterationsLimit(1e6);
[xtraj,info] = inverseKinTraj(r,t_plan,qtraj0,qtraj0,cnstr{:},r_hand_cnstr{:},ikoptions);

to increase the iterations limit of SNOPT. Notice I also use 10 knot points instead of 20. I think 20 knot points make the problem too big, and you will probably see too much over-damping in your solution when use 20 knot points.

[AndreasMegatranus]

OK, I'll try that. Is there something especially difficult about this
problem? It seems like it should be easy to solve, and there shouldn't be constraint violations, even
at points between the knots. There shouldn't have to be a lot of knots. I'm just trying to move the hand up in the z direction a few
centimeters. Is the initial pose near a singularity? Or is the problem that the initial guess trajectory
given to snopt is not a very good guess?

Are there any examples in Drake similar to what I'm trying to do?
Just a simple move of a robot hand in one direction, a few centimeters?

Andreas

Quoting Hongkai Dai notifications@github.com:

I got info 32 when running your code, which means the major
iteration limit is reached, and the solver fails to solve the
problem. That is why the constraints are violated. You can try

t_plan = linspace(0,1,10);
ikoptions = IKoptions(r);
ikoptions = ikoptions.setMajorIterationsLimit(1e4);
ikoptions = ikoptions.setIterationsLimit(1e6);
[xtraj,info] =  
inverseKinTraj(r,t_plan,qtraj0,qtraj0,cnstr{:},r_hand_cnstr{:},ikoptions);

to increase the iterations limit of SNOPT. Notice I also use 10 knot
points instead of 20. I think 20 knot points make the problem too
big, and you will probably see too much over-damping in your
solution when use 20 knot points.

---

You are receiving this because you authored the thread. Reply to this email directly or view it on GitHub: https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-207446322

[AndreasMegatranus]

I just realized one thing I was doing wrong. For the
WorldPositionConstraint, I was using [0,0,0] as the point, but for the forward kinematics check, I was using
r_hand_pt ([0, -0.2, 0]) as the point. I fixed this to use r_hand_pt for both, and using 5 knots in t. Now,
inverseKinTraj returns 1 instead of 32, and all constraints are satisfied at the knot points.

Plan execution still looks weird, and then crashes later on during
visualization. Will investigate some more.

Andreas

Quoting Hongkai Dai notifications@github.com:

When you call inverseKinTraj, what value do you pass to the t
argument, and did you call function
IKoptions.addAdditionaltSamples? inverseKinTraj constructs a
cubic spline, it uses the argument t as the knot points for the
cubic spline, and it guarantees that the constraints are satisfied
at t and the additional t samples (the argument passed in from
IKoptions.addAdditionaltSamples). So if you want your constraints to
be satisfied at t_breaks = linspace(t_plan(1),t_plan(end),n_breaks);, you can call

options = IKoptions(r);
options = options.addAdditionaltSamples(t_breaks);
[xtraj,info] = inverseKinTraj(r,t,q_seed_traj,q_nom_traj,CONSTRAINT,options)

Let me know if you still find the constraints being violated

---

You are receiving this because you authored the thread. Reply to this email directly or view it on GitHub: https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-206897232

[AndreasMegatranus]

Hello Hongkai,

After the fix to use consistent points, the inverseKinTraj seems to be
working well. I added some nifty plots that show that the x, y, roll, pitch, and yaw trajectories
remain essentially constant, and z moves smoothly in a nice S curve to the goal position (see
attached file).

However, further on down, when it gets to executing the trajectory

traj = simulate(sys,[0 T],x0);

it seems, from the visualization, to do something very different. It
looks very unstable; the hand spins around, the body bends, etc. It does get through this step
saying "current plan is finished".

Why is this happening? Is it a problem with control?

Andreas

Quoting Hongkai Dai notifications@github.com:

When you call inverseKinTraj, what value do you pass to the t
argument, and did you call function
IKoptions.addAdditionaltSamples? inverseKinTraj constructs a
cubic spline, it uses the argument t as the knot points for the
cubic spline, and it guarantees that the constraints are satisfied
at t and the additional t samples (the argument passed in from
IKoptions.addAdditionaltSamples). So if you want your constraints to
be satisfied at t_breaks = linspace(t_plan(1),t_plan(end),n_breaks);, you can call

options = IKoptions(r);
options = options.addAdditionaltSamples(t_breaks);
[xtraj,info] = inverseKinTraj(r,t,q_seed_traj,q_nom_traj,CONSTRAINT,options)

Let me know if you still find the constraints being violated

---

You are receiving this because you authored the thread. Reply to this email directly or view it on GitHub: https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-206897232

function runAtlasHandControl(example_options)

% Tests out the piping to add a robotiq hand (with kinematic loops) % to Atlas, and hook up a controller to command its joints

if ~checkDependency('gurobi') warning('Must have gurobi installed to run this example'); return; end

import bipedControllers.*

path_handle = addpathTemporary(fullfile(getDrakePath(), 'examples', 'ZMP'));

% put robot in a random x,y,yaw position and balance for 2 seconds if nargin<1, example_options=struct(); end example_options = applyDefaults(example_options, struct('use_mex', true, ... 'use_bullet', false,... 'visualize', true,... 'terrain', RigidBodyFlatTerrain));

if (nargin<1); use_mex = true; end if (nargin<2); use_angular_momentum = false; end

% silence some warnings warning('off','Drake:RigidBodyManipulator:UnsupportedContactPoints') warning('off','Drake:RigidBodyManipulator:UnsupportedVelocityLimits')

options.floating = true; options.ignore_self_collisions = true; options.ignore_friction = true; options.dt = 0.001; options.terrain = example_options.terrain; options.use_bullet = example_options.use_bullet; options.use_mex = example_options.use_mex; options.hand_left = 'robotiq_weight_only'; options.hand_right = 'robotiq_weight_only'; r = Atlas('urdf/atlas_minimal_contact.urdf',options); options.hand_left = 'robotiq_tendons'; options.hand_right = 'robotiq_tendons'; r_hands = Atlas('urdf/atlas_minimal_contact.urdf',options); r = r.removeCollisionGroupsExcept({'heel','toe'}); r_hands = r_hands.removeCollisionGroupsExcept({'heel','toe'}); r = compile(r); r_hands = compile(r_hands);

nq = getNumPositions(r); nv = getNumVelocities(r);

keyboard

% set initial state to fixed point load('data/atlas_fp.mat'); xstar(1) = 0; %0.1_randn(); xstar(2) = 0; %0.1_randn(); xstar(6) = 0;% pi*randn(); r = r.setInitialState(xstar);

keyboard

% and hand state to a feasible sol % so lcp has an easier time %load('data/robotiq_feas.mat'); xstar_hands = r_hands.getInitialState(); xstar_hands(1:length(xstar)) = xstar; %xstar_hands = r_hands.getStateFrame().mergeCoordinates({xstar,xstar_hand}); xstar_hands = r_hands.resolveConstraints(xstar_hands); r_hands = r_hands.setInitialState(xstar_hands);

keyboard

x0 = xstar; q0 = x0(1:nq); v0 = x0(nq+(1:nv)); pelvis = r.findLinkId('pelvis'); cnstr = fix_feet_cnstr(r,x0); r_hand = r.findLinkId('r_hand'); r_hand_pt = [0;-0.2;0]; % r_hand_pt = [0;0;0]; kinsol0 = r.doKinematics(q0,v0); r_hand_pos0 = r.forwardKin(kinsol0,r_hand,r_hand_pt,2);

rpy_r_hand_pos0 = quat2rpy(r_hand_pos0(4:7));

r_hand_pos0_euler = r.forwardKin(kinsol0,r_hand,r_hand_pt,1);

% r_hand_cnstr = {WorldEulerConstraint(r,r_hand,[nan;pi/2;nan],[nan;pi/2;nan],[0.3 0.7]),... % WorldEulerConstraint(r,r_hand,[nan;pi/4;nan],[nan;0.6*pi;nan],[1,1])};

% Include position as well as orientation constraints.

x_lb_op = 0.2; x_ub_op = 0.36; y_lb_op = -0.44; y_ub_op = -0.38; z_lb_op = 0.51; z_ub_op = 0.55;

roll_lb_op = 1.0; roll_ub_op = 1.4; pitch_lb_op = 0; pitch_ub_op = 0.4; yaw_lb_op = 1.8; yaw_ub_op = 2.2;

goal_position_constraint = WorldPositionConstraint(r,r_hand,r_hand_pt,[0.2832; -0.4185; 0.5360], ... [0.2832; -0.4185; 0.5460],[1,1]);

goal_euler_constraint = WorldEulerConstraint(r,r_hand,[nan;pi/4;nan],[nan;0.6*pi;nan],[1,1]);

operation_position_constraint = WorldPositionConstraint(r,r_hand,r_hand_pt,[x_lb_op; y_lb_op; z_lb_op], ... [x_ub_op; y_ub_op; z_ub_op],[0,1]); % Kinematic flow tube.

operation_euler_constraint = WorldEulerConstraint(r,r_hand,[roll_lb_op;pitch_lb_op;yaw_lb_op],[roll_ub_op;pitch_ub_op;yaw_ub_op],[0,1]);

r_hand_cnstr = {goal_position_constraint, operation_euler_constraint, operation_position_constraint};

% Really do need orientation constraints!

qtraj0 = PPTrajectory(zoh([0,1],[q0 q0]));

% t_plan = [0,0.5,1];

t_plan = [0:0.2:1]; % Try more knot points.

n_breaks = 5; t_breaks = linspace(t_plan(1),t_plan(end),n_breaks);

t_plan = t_breaks;

[xtraj,info] = inverseKinTraj(r,t_plan,qtraj0,qtraj0,cnstr{:},r_hand_cnstr{:});

keyboard

x_breaks = xtraj.eval(t_breaks);

kinsol_x_breaks_1 = r.doKinematics(x_breaks(1:36,1), x_breaks(37:72,1)); r_hand_pos_x_breaks_1 = r.forwardKin(kinsol_x_breaks_1,r_hand,r_hand_pt,1);

kinsol_x_breaks_2 = r.doKinematics(x_breaks(1:36,2), x_breaks(37:72,2)); r_hand_pos_x_breaks_2 = r.forwardKin(kinsol_x_breaks_2,r_hand,r_hand_pt,1);

kinsol_x_breaks_5 = r.doKinematics(x_breaks(1:36,5), x_breaks(37:72,5)); r_hand_pos_x_breaks_5 = r.forwardKin(kinsol_x_breaks_5,r_hand,r_hand_pt,1);

% kinsol_x_breaks_20 = r.doKinematics(x_breaks(1:36,20), x_breaks(37:72,20)); % r_hand_pos_x_breaks_20 = r.forwardKin(kinsol_x_breaks_20,r_hand,r_hand_pt,1);

keyboard

n_breaks2 = 20; t_breaks2 = linspace(t_plan(1),t_plan(end),n_breaks2); x_breaks2 = xtraj.eval(t_breaks2);

r_hand_pos_traj = [];

for idx = 1:20 kinsol_x_breaks = r.doKinematics(x_breaks2(1:36,idx), x_breaks2(37:72,idx)); r_hand_pos = r.forwardKin(kinsol_x_breaks,r_hand,r_hand_pt,1); r_hand_pos_traj = [r_hand_pos_traj, r_hand_pos]; end

figure; plot(t_breaks2, r_hand_pos_traj(1,:)); hold on; plot([0,1],[x_lb_op, x_lb_op], 'r'); plot([0,1],[x_ub_op, x_ub_op], 'r'); title('x');

figure; plot(t_breaks2, r_hand_pos_traj(2,:)); hold on; plot([0,1],[y_lb_op, y_lb_op], 'r'); plot([0,1],[y_ub_op, y_ub_op], 'r'); title('y');

figure; plot(t_breaks2, r_hand_pos_traj(3,:)); hold on; plot([0,1],[z_lb_op, z_lb_op], 'r'); plot([0,1],[z_ub_op, z_ub_op], 'r'); title('z');

figure; plot(t_breaks2, r_hand_pos_traj(4,:)); hold on; plot([0,1],[roll_lb_op, roll_lb_op], 'r'); plot([0,1],[roll_ub_op, roll_ub_op], 'r'); title('roll');

figure; plot(t_breaks2, r_hand_pos_traj(5,:)); hold on; plot([0,1],[pitch_lb_op, pitch_lb_op], 'r'); plot([0,1],[pitch_ub_op, pitch_ub_op], 'r'); title('pitch');

figure; plot(t_breaks2, r_hand_pos_traj(5,:)); hold on; plot([0,1],[yaw_lb_op, yaw_lb_op], 'r'); plot([0,1],[yaw_ub_op, yaw_ub_op], 'r'); title('yaw');

keyboard

t_breaks = t_breaks_4; x_breaks(nq+(1:nv),:) = 1/4_x_breaks(nq+(1:nv),:); qtraj_pp = spline(t_breaks,[zeros(nq,1) x_breaks(1:nq,:) zeros(nq,1)]);

manip_plan_data = QPLocomotionPlanSettings.fromQuasistaticQTraj(r,PPTrajectory(qtraj_pp),... struct('bodies_to_track',[pelvis,r_hand],... 'quat_task_to_world',repmat(uniformlyRandomQuat(),1,2),... 'translation_task_to_world',randn(3,2),... 'is_quasistatic',true,... 'gain_set', 'manip')); r_arm_idx = r.findPositionIndices('r_arm');

manip_plan_data.untracked_joint_inds = r_arm_idx; control = InstantaneousQPController(r, []); planeval = BipedPlanEval(r, QPLocomotionPlanCPPWrapper(manip_plan_data)); plancontroller = BipedPlanEvalAndControlSystem(r, control, planeval);

ins(1).system = 2; ins(1).input = 2; ins(2).system = 2; ins(2).input = 3; outs(1).system = 2; outs(1).output = 1; outs(2).system = 2; outs(2).output = 2; outs(3).system = 2; outs(3).output = 3; sys = mimoFeedback(plancontroller,r_hands,[],[],ins,outs); clear ins; clear outs;

ins(1).system = 1; ins(1).input = 2; outs(1).system = 1; outs(1).output = 1; outs(2).system = 1; outs(2).output = 2; outs(3).system = 1; outs(3).output = 3;

rh = RobotiqControlBlock(rhands, 2, 'right'); sys = mimoFeedback(sys, rh, [], [], ins, outs);

lh = RobotiqControlBlock(rhands, 3, 'left'); sys = mimoFeedback(sys, lh, [], [], [], outs);

keyboard

if example_options.visualize v = r_hands.constructVisualizer; v.display_dt = 0.01; S=warning('off','Drake:DrakeSystem:UnsupportedSampleTime'); output_select(1).system=1; output_select(1).output=1; output_select(2).system=1; output_select(2).output=2; output_select(3).system = 1; output_select(3).output = 3; sys = mimoCascade(sys,v,[],[],output_select); warning(S); end x0 = xstar_hands; T = min(manip_plan_data.duration + 1, 30); traj = simulate(sys,[0 T],x0); if example_options.visualize % This doesn't see hand movements. Why? playback(v,traj,struct('slider',true)); end

%xf = traj.eval(traj.tspan(2));

function cnstr = fix_feet_cnstr(r,x0) q0 = x0(1:r.getNumPositions); v0 = x0(r.getNumPositions+(1:r.getNumVelocities)); kinsol0 = r.doKinematics(q0,v0,false); l_foot = r.findLinkId('l_foot'); r_foot = r.findLinkId('r_foot'); l_foot_pts = r.getBody(l_foot).getTerrainContactPoints; r_foot_pts = r.getBody(r_foot).getTerrainContactPoints; l_foot_pos0 = r.forwardKin(kinsol0,l_foot,zeros(3,1),2); r_foot_pos0 = r.forwardKin(kinsol0,r_foot,zeros(3,1),2); lfoot_cnstr = {WorldPositionConstraint(r,l_foot,zeros(3,1),l_foot_pos0(1:3),l_foot_pos0(1:3)),... WorldQuatConstraint(r,l_foot,l_foot_pos0(4:7),0)}; rfoot_cnstr = {WorldPositionConstraint(r,r_foot,zeros(3,1),r_foot_pos0(1:3),r_foot_pos0(1:3)),... WorldQuatConstraint(r,r_foot,r_foot_pos0(4:7),0)}; qsc = QuasiStaticConstraint(r,[-inf,inf],1); qsc = qsc.addContact(l_foot,l_foot_pts,r_foot,r_foot_pts); qsc = qsc.setShrinkFactor(0.6); qsc = qsc.setActive(true); utorso = r.findLinkId('utorso'); utorso_upright = WorldGazeDirConstraint(r,utorso,[0;0;1],[0;0;1],0.05*pi); cnstr = [lfoot_cnstr,rfoot_cnstr,{qsc,utorso_upright}]; end

end

[hongkai-dai]

The controller does a lot more things than tracking the position/orientation of the hand (for example, to balance the robot). It would help if you could let me know the branch on which your code lives, and then probably I can run it on my machine.

[AndreasMegatranus]

I didn't make a branch, but I sent you the runAtlasHandControl.m file
(this is the only file that I modified). I suspect that you'll get the same behavior if you just run this file.
Also, I will be at MIT CSAIL on Wed., I could come over to your lab and show you what's happening on my laptop.

Andreas

Andreas

Quoting Hongkai Dai notifications@github.com:

The controller does a lot more things than tracking the
position/orientation of the hand (for example, to balance the
robot). It would help if you could let me know the branch on which
your code lives, and then probably I can run it on my machine.

---

You are receiving this because you authored the thread. Reply to this email directly or view it on GitHub: https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-208523143

[hongkai-dai]

Could you send me the most updated version of the file? You mentioned you made some changes these days.

On Mon, Apr 11, 2016 at 4:12 PM, AndreasMegatranus <notifications@github.com

wrote:

I didn't make a branch, but I sent you the runAtlasHandControl.m file (this is the only file that I modified). I suspect that you'll get the same behavior if you just run this file. Also, I will be at MIT CSAIL on Wed., I could come over to your lab and show you what's happening on my laptop.

Andreas

Andreas

Quoting Hongkai Dai notifications@github.com:

The controller does a lot more things than tracking the position/orientation of the hand (for example, to balance the robot). It would help if you could let me know the branch on which your code lives, and then probably I can run it on my machine.

---

You are receiving this because you authored the thread. Reply to this email directly or view it on GitHub:

https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-208523143

— You are receiving this because you were assigned. Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-208537373

[AndreasMegatranus]

Here it is.

Quoting Hongkai Dai notifications@github.com:

Could you send me the most updated version of the file? You mentioned you made some changes these days.

On Mon, Apr 11, 2016 at 4:12 PM, AndreasMegatranus <notifications@github.com

wrote:

I didn't make a branch, but I sent you the runAtlasHandControl.m file (this is the only file that I modified). I suspect that you'll get the same behavior if you just run this file. Also, I will be at MIT CSAIL on Wed., I could come over to your lab and show you what's happening on my laptop.

Andreas

Andreas

Quoting Hongkai Dai notifications@github.com:

The controller does a lot more things than tracking the position/orientation of the hand (for example, to balance the robot). It would help if you could let me know the branch on which your code lives, and then probably I can run it on my machine.

---

You are receiving this because you authored the thread. Reply to this email directly or view it on GitHub:

https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-208523143

— You are receiving this because you were assigned. Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-208537373

---

You are receiving this because you authored the thread. Reply to this email directly or view it on GitHub: https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-208553367

function runAtlasHandControl(example_options)

% Tests out the piping to add a robotiq hand (with kinematic loops) % to Atlas, and hook up a controller to command its joints

if ~checkDependency('gurobi') warning('Must have gurobi installed to run this example'); return; end

import bipedControllers.*

path_handle = addpathTemporary(fullfile(getDrakePath(), 'examples', 'ZMP'));

% put robot in a random x,y,yaw position and balance for 2 seconds if nargin<1, example_options=struct(); end example_options = applyDefaults(example_options, struct('use_mex', true, ... 'use_bullet', false,... 'visualize', true,... 'terrain', RigidBodyFlatTerrain));

if (nargin<1); use_mex = true; end if (nargin<2); use_angular_momentum = false; end

% silence some warnings warning('off','Drake:RigidBodyManipulator:UnsupportedContactPoints') warning('off','Drake:RigidBodyManipulator:UnsupportedVelocityLimits')

options.floating = true; options.ignore_self_collisions = true; options.ignore_friction = true; options.dt = 0.001; options.terrain = example_options.terrain; options.use_bullet = example_options.use_bullet; options.use_mex = example_options.use_mex; options.hand_left = 'robotiq_weight_only'; options.hand_right = 'robotiq_weight_only'; r = Atlas('urdf/atlas_minimal_contact.urdf',options); options.hand_left = 'robotiq_tendons'; options.hand_right = 'robotiq_tendons'; r_hands = Atlas('urdf/atlas_minimal_contact.urdf',options); r = r.removeCollisionGroupsExcept({'heel','toe'}); r_hands = r_hands.removeCollisionGroupsExcept({'heel','toe'}); r = compile(r); r_hands = compile(r_hands);

nq = getNumPositions(r); nv = getNumVelocities(r);

keyboard

% set initial state to fixed point load('data/atlas_fp.mat'); xstar(1) = 0; %0.1_randn(); xstar(2) = 0; %0.1_randn(); xstar(6) = 0;% pi*randn(); r = r.setInitialState(xstar);

keyboard

% and hand state to a feasible sol % so lcp has an easier time %load('data/robotiq_feas.mat'); xstar_hands = r_hands.getInitialState(); xstar_hands(1:length(xstar)) = xstar; %xstar_hands = r_hands.getStateFrame().mergeCoordinates({xstar,xstar_hand}); xstar_hands = r_hands.resolveConstraints(xstar_hands); r_hands = r_hands.setInitialState(xstar_hands);

keyboard

x0 = xstar; q0 = x0(1:nq); v0 = x0(nq+(1:nv)); pelvis = r.findLinkId('pelvis'); cnstr = fix_feet_cnstr(r,x0); r_hand = r.findLinkId('r_hand'); r_hand_pt = [0;-0.2;0]; % r_hand_pt = [0;0;0]; kinsol0 = r.doKinematics(q0,v0); r_hand_pos0 = r.forwardKin(kinsol0,r_hand,r_hand_pt,2);

rpy_r_hand_pos0 = quat2rpy(r_hand_pos0(4:7));

r_hand_pos0_euler = r.forwardKin(kinsol0,r_hand,r_hand_pt,1);

% r_hand_cnstr = {WorldEulerConstraint(r,r_hand,[nan;pi/2;nan],[nan;pi/2;nan],[0.3 0.7]),... % WorldEulerConstraint(r,r_hand,[nan;pi/4;nan],[nan;0.6*pi;nan],[1,1])};

% Include position as well as orientation constraints.

x_lb_op = 0.2; x_ub_op = 0.36; y_lb_op = -0.44; y_ub_op = -0.38; z_lb_op = 0.51; z_ub_op = 0.55;

roll_lb_op = 1.0; roll_ub_op = 1.4; pitch_lb_op = 0; pitch_ub_op = 0.4; yaw_lb_op = 1.8; yaw_ub_op = 2.2;

goal_position_constraint = WorldPositionConstraint(r,r_hand,r_hand_pt,[0.2832; -0.4185; 0.5360], ... [0.2832; -0.4185; 0.5460],[1,1]);

goal_euler_constraint = WorldEulerConstraint(r,r_hand,[nan;pi/4;nan],[nan;0.6*pi;nan],[1,1]);

operation_position_constraint = WorldPositionConstraint(r,r_hand,r_hand_pt,[x_lb_op; y_lb_op; z_lb_op], ... [x_ub_op; y_ub_op; z_ub_op],[0,1]); % Kinematic flow tube.

operation_euler_constraint = WorldEulerConstraint(r,r_hand,[roll_lb_op;pitch_lb_op;yaw_lb_op],[roll_ub_op;pitch_ub_op;yaw_ub_op],[0,1]);

r_hand_cnstr = {goal_position_constraint, operation_euler_constraint, operation_position_constraint};

% Really do need orientation constraints!

qtraj0 = PPTrajectory(zoh([0,1],[q0 q0]));

% t_plan = [0,0.5,1];

t_plan = [0:0.2:1]; % Try more knot points.

n_breaks = 5; t_breaks = linspace(t_plan(1),t_plan(end),n_breaks);

t_plan = t_breaks;

[xtraj,info] = inverseKinTraj(r,t_plan,qtraj0,qtraj0,cnstr{:},r_hand_cnstr{:});

keyboard

x_breaks = xtraj.eval(t_breaks);

kinsol_x_breaks_1 = r.doKinematics(x_breaks(1:36,1), x_breaks(37:72,1)); r_hand_pos_x_breaks_1 = r.forwardKin(kinsol_x_breaks_1,r_hand,r_hand_pt,1);

kinsol_x_breaks_2 = r.doKinematics(x_breaks(1:36,2), x_breaks(37:72,2)); r_hand_pos_x_breaks_2 = r.forwardKin(kinsol_x_breaks_2,r_hand,r_hand_pt,1);

kinsol_x_breaks_5 = r.doKinematics(x_breaks(1:36,5), x_breaks(37:72,5)); r_hand_pos_x_breaks_5 = r.forwardKin(kinsol_x_breaks_5,r_hand,r_hand_pt,1);

% kinsol_x_breaks_20 = r.doKinematics(x_breaks(1:36,20), x_breaks(37:72,20)); % r_hand_pos_x_breaks_20 = r.forwardKin(kinsol_x_breaks_20,r_hand,r_hand_pt,1);

keyboard

n_breaks2 = 20; t_breaks2 = linspace(t_plan(1),t_plan(end),n_breaks2); x_breaks2 = xtraj.eval(t_breaks2);

r_hand_pos_traj = [];

for idx = 1:20 kinsol_x_breaks = r.doKinematics(x_breaks2(1:36,idx), x_breaks2(37:72,idx)); r_hand_pos = r.forwardKin(kinsol_x_breaks,r_hand,r_hand_pt,1); r_hand_pos_traj = [r_hand_pos_traj, r_hand_pos]; end

figure; plot(t_breaks2, r_hand_pos_traj(1,:)); hold on; plot([0,1],[x_lb_op, x_lb_op], 'r'); plot([0,1],[x_ub_op, x_ub_op], 'r'); title('x');

figure; plot(t_breaks2, r_hand_pos_traj(2,:)); hold on; plot([0,1],[y_lb_op, y_lb_op], 'r'); plot([0,1],[y_ub_op, y_ub_op], 'r'); title('y');

figure; plot(t_breaks2, r_hand_pos_traj(3,:)); hold on; plot([0,1],[z_lb_op, z_lb_op], 'r'); plot([0,1],[z_ub_op, z_ub_op], 'r'); title('z');

figure; plot(t_breaks2, r_hand_pos_traj(4,:)); hold on; plot([0,1],[roll_lb_op, roll_lb_op], 'r'); plot([0,1],[roll_ub_op, roll_ub_op], 'r'); title('roll');

figure; plot(t_breaks2, r_hand_pos_traj(5,:)); hold on; plot([0,1],[pitch_lb_op, pitch_lb_op], 'r'); plot([0,1],[pitch_ub_op, pitch_ub_op], 'r'); title('pitch');

figure; plot(t_breaks2, r_hand_pos_traj(5,:)); hold on; plot([0,1],[yaw_lb_op, yaw_lb_op], 'r'); plot([0,1],[yaw_ub_op, yaw_ub_op], 'r'); title('yaw');

keyboard

t_breaks = t_breaks_4; x_breaks(nq+(1:nv),:) = 1/4_x_breaks(nq+(1:nv),:); qtraj_pp = spline(t_breaks,[zeros(nq,1) x_breaks(1:nq,:) zeros(nq,1)]);

manip_plan_data = QPLocomotionPlanSettings.fromQuasistaticQTraj(r,PPTrajectory(qtraj_pp),... struct('bodies_to_track',[pelvis,r_hand],... 'quat_task_to_world',repmat(uniformlyRandomQuat(),1,2),... 'translation_task_to_world',randn(3,2),... 'is_quasistatic',true,... 'gain_set', 'manip')); r_arm_idx = r.findPositionIndices('r_arm');

manip_plan_data.untracked_joint_inds = r_arm_idx; control = InstantaneousQPController(r, []); planeval = BipedPlanEval(r, QPLocomotionPlanCPPWrapper(manip_plan_data)); plancontroller = BipedPlanEvalAndControlSystem(r, control, planeval);

ins(1).system = 2; ins(1).input = 2; ins(2).system = 2; ins(2).input = 3; outs(1).system = 2; outs(1).output = 1; outs(2).system = 2; outs(2).output = 2; outs(3).system = 2; outs(3).output = 3; sys = mimoFeedback(plancontroller,r_hands,[],[],ins,outs); clear ins; clear outs;

ins(1).system = 1; ins(1).input = 2; outs(1).system = 1; outs(1).output = 1; outs(2).system = 1; outs(2).output = 2; outs(3).system = 1; outs(3).output = 3;

rh = RobotiqControlBlock(rhands, 2, 'right'); sys = mimoFeedback(sys, rh, [], [], ins, outs);

lh = RobotiqControlBlock(rhands, 3, 'left'); sys = mimoFeedback(sys, lh, [], [], [], outs);

keyboard

if example_options.visualize v = r_hands.constructVisualizer; v.display_dt = 0.01; S=warning('off','Drake:DrakeSystem:UnsupportedSampleTime'); output_select(1).system=1; output_select(1).output=1; output_select(2).system=1; output_select(2).output=2; output_select(3).system = 1; output_select(3).output = 3; sys = mimoCascade(sys,v,[],[],output_select); warning(S); end x0 = xstar_hands; T = min(manip_plan_data.duration + 1, 30); traj = simulate(sys,[0 T],x0); if example_options.visualize % This doesn't see hand movements. Why? playback(v,traj,struct('slider',true)); end

%xf = traj.eval(traj.tspan(2));

function cnstr = fix_feet_cnstr(r,x0) q0 = x0(1:r.getNumPositions); v0 = x0(r.getNumPositions+(1:r.getNumVelocities)); kinsol0 = r.doKinematics(q0,v0,false); l_foot = r.findLinkId('l_foot'); r_foot = r.findLinkId('r_foot'); l_foot_pts = r.getBody(l_foot).getTerrainContactPoints; r_foot_pts = r.getBody(r_foot).getTerrainContactPoints; l_foot_pos0 = r.forwardKin(kinsol0,l_foot,zeros(3,1),2); r_foot_pos0 = r.forwardKin(kinsol0,r_foot,zeros(3,1),2); lfoot_cnstr = {WorldPositionConstraint(r,l_foot,zeros(3,1),l_foot_pos0(1:3),l_foot_pos0(1:3)),... WorldQuatConstraint(r,l_foot,l_foot_pos0(4:7),0)}; rfoot_cnstr = {WorldPositionConstraint(r,r_foot,zeros(3,1),r_foot_pos0(1:3),r_foot_pos0(1:3)),... WorldQuatConstraint(r,r_foot,r_foot_pos0(4:7),0)}; qsc = QuasiStaticConstraint(r,[-inf,inf],1); qsc = qsc.addContact(l_foot,l_foot_pts,r_foot,r_foot_pts); qsc = qsc.setShrinkFactor(0.6); qsc = qsc.setActive(true); utorso = r.findLinkId('utorso'); utorso_upright = WorldGazeDirConstraint(r,utorso,[0;0;1],[0;0;1],0.05*pi); cnstr = [lfoot_cnstr,rfoot_cnstr,{qsc,utorso_upright}]; end

end

[AndreasMegatranus]

Could it be that I'm just trying to move the arm too fast?

Quoting Hongkai Dai notifications@github.com:

Could you send me the most updated version of the file? You mentioned you made some changes these days.

On Mon, Apr 11, 2016 at 4:12 PM, AndreasMegatranus <notifications@github.com

wrote:

I didn't make a branch, but I sent you the runAtlasHandControl.m file (this is the only file that I modified). I suspect that you'll get the same behavior if you just run this file. Also, I will be at MIT CSAIL on Wed., I could come over to your lab and show you what's happening on my laptop.

Andreas

Andreas

Quoting Hongkai Dai notifications@github.com:

The controller does a lot more things than tracking the position/orientation of the hand (for example, to balance the robot). It would help if you could let me know the branch on which your code lives, and then probably I can run it on my machine.

---

You are receiving this because you authored the thread. Reply to this email directly or view it on GitHub:

https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-208523143

— You are receiving this because you were assigned. Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-208537373

---

You are receiving this because you authored the thread. Reply to this email directly or view it on GitHub: https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-208553367

[hongkai-dai]

Sorry for the belated reply. But I could not run your file. I get the following error

file.
What() is:mxGetStdString failed.
Possible cause: mxArray is not a string
array.
..

Error in
bipedControllers.InstantaneousQPController
(line 22)
      obj.data_mex_ptr = ...

Error in runAtlasHandControl (line 230)
control = InstantaneousQPController(r,
[]);

>> 
>> 

I think this is probably because you are running an old version of drake. Could you let me know the SHA of your the latest commit to your drake? You could find it using command

cd DRAKE_FOLDER
git log

in the terminal

[AndreasMegatranus]

Here are the first few lines after doing git log:

commit 6a54022ee4a8061299868c7db228898d165a7911 Merge: 5c74449 7476c39 Author: Russ Tedrake russt@mit.edu Date: Wed Jan 20 15:56:09 2016 -0500

 Merge pull request #1635 from patmarion/update-signal-scope

 update signal-scope external sha1

Quoting Hongkai Dai notifications@github.com:

Sorry for the belated reply. But I could not run your file. I get
the following error

file.
What() is:mxGetStdString failed.
Possible cause: mxArray is not a string
array.
..

Error in
bipedControllers.InstantaneousQPController
(line 22)
      obj.data_mex_ptr = ...

Error in runAtlasHandControl (line 230)
control = InstantaneousQPController(r,
[]);

>>
>>

I think this is probably because you are running an old version of
drake. Could you let me know the SHA of your the latest commit to
your drake? You could find it using command

cd DRAKE_FOLDER
git log

in the terminal

---

You are receiving this because you authored the thread. Reply to this email directly or view it on GitHub: https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-209019692

[hongkai-dai]

@rdeits and I looked at this bug together today. We also observe strange behavior in the controller. It looks like the exponential map wrap up error we had before. I also tried to remove the tracking on the right hand, and track the right arm motion instead, and I got relatively good tracking on the right hand position in the simulation, but poor tracking on the right hand orientation. This could be a problem on our bodySpatialPDcontroller block. I will look into it.

[AndreasMegatranus]

OK, thanks for the update. Are there any examples of Atlas arm
movement that work? There must be; you must have used something for the DARPA robotics challenge. Anyway, if there aren't any readily available examples, I'll just wait
for you to look into this.

Andreas

Quoting Hongkai Dai notifications@github.com:

@rdeits and I looked at this bug together today. We also observe
strange behavior in the controller. It looks like the exponential
map wrap up error we had before. I also tried to remove the tracking
on the right hand, and track the right arm motion instead, and I got
relatively good tracking on the right hand position in the
simulation, but poor tracking on the right hand orientation. This
could be a problem on our bodySpatialPDcontroller block. I will look
into it.

---

You are receiving this because you authored the thread. Reply to this email directly or view it on GitHub: https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-209190320

[hongkai-dai]

You can look at the runAtlasManip in the same folder

[AndreasMegatranus]

I just tried to run that. It gets an error when bringing up the visualizer:

attempting to launch the drake director Error using BotVisualizer (line 48) Failed to automatically start up a viewer (or to receive the ack, see https://github.com/RobotLocomotion/drake/issues/317)

Error in RigidBodyManipulator/constructVisualizer (line 1708) v = feval(type,arg{:});

Quoting Hongkai Dai notifications@github.com:

You can look at the runAtlasManip in the same folder

---

You are receiving this because you authored the thread. Reply to this email directly or view it on GitHub: https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-209198528

[hongkai-dai]

Could you update to the the newest version of drake? After the update and you have built Drake, in the terminal, could you do

cd DRAKE_FOLDER/build/bin
./drake-visualizer

[AndreasMegatranus]

How do I update? Should I do a git pull on all of Drake? Then the
usual build? I don't want to do anything that will mess up all the third party
solvers I've managed to get working with Drake recently (gurobi, snopt, etc.).

Andreas

Quoting Hongkai Dai notifications@github.com:

Could you update to the the newest version of drake? After the
update and you have built Drake, in the terminal, could you do

cd DRAKE_FOLDER/build/bin
./drake-visualizer

---

You are receiving this because you authored the thread. Reply to this email directly or view it on GitHub: https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-209534569

[hongkai-dai]

Did you install these third party solvers by yourself, or drake install them for you? If Drake installs these third party solvers, then after the update, they should still work. If you have made local changes to drake, you can stash them by git stash, and then update drake by git pull, and then apply your stashed change using git stash apply.

Before you do the git update, could you launch the viewer from terminal, by doing

cd DRAKE_FOLDER/build/bin
./drake-visualizer

If you see the drake visualizer pops up, then you probably do not need to update drake.

When you saw the viewer error before, did you have internet connection? LCM can fail if you shut down the network on your computer.

[AndreasMegatranus]

../drake-visualizer works

Furthermore, runAtlasManip is suddenly working. I do get the following LCM warnings:

LCM: Disabling IPV6 support LCM: TTL set to zero, traffic will not leave localhost.

I do have internet connection.

runAtlasManip works in that it generates the plan and executes the
visualization. However, the motion that is executed is very jerky, including a
stepping motion that the robot has to take, apparently to maintain balance. Is it moving
through a singularity somehow?

I'll experiment with this example, and try to make a simple arm motion
like I did in runAtlasHandControl.

Regarding the solvers, I believe I had drake install them for me. I
haven't made any local changes to drake other than the (minor) modifications to
runAtlasHandControl (which I can copy into my own version of this file).

Quoting Hongkai Dai notifications@github.com:

Did you install these third party solvers by yourself, or drake
install them for you? If Drake installs these third party solvers,
then after the update, they should still work. If you have made
local changes to drake, you can stash them by git stash, and then
update drake by git pull, and then apply your stashed change using
git stash apply.

Before you do the git update, could you launch the viewer from
terminal, by doing

cd DRAKE_FOLDER/build/bin
./drake-visualizer

If you see the drake visualizer pops up, then you probably do not
need to update drake.

When you saw the viewer error before, did you have internet
connection? LCM can fail if you shut down the network on your
computer.

---

You are receiving this because you authored the thread. Reply to this email directly or view it on GitHub: https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-209652432

[AndreasMegatranus]

I am now able to do simple arm movements using runAtlasManip. The end
effector moves smoothly. It does look like some of the upper joints are doing
something funny when moving from the initial state; I assume that's due to a singularity.

I've been looking at the call

v = r.constructVisualizer

Is it possible to put two robots in the visualizer?

Andreas

Quoting Hongkai Dai notifications@github.com:

Did you install these third party solvers by yourself, or drake
install them for you? If Drake installs these third party solvers,
then after the update, they should still work. If you have made
local changes to drake, you can stash them by git stash, and then
update drake by git pull, and then apply your stashed change using
git stash apply.

Before you do the git update, could you launch the viewer from
terminal, by doing

cd DRAKE_FOLDER/build/bin
./drake-visualizer

If you see the drake visualizer pops up, then you probably do not
need to update drake.

When you saw the viewer error before, did you have internet
connection? LCM can fail if you shut down the network on your
computer.

---

You are receiving this because you authored the thread. Reply to this email directly or view it on GitHub: https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-209652432

[hongkai-dai]

Yes, you can put two robots into r, by calling addRobotFromURDF, addRobotFromSDF or addRobotFromURDFString. One example is

r = RigidBodyManipulator([getDrakePath,'/examples/Atlas/urdf/atlas_minimal_contact.urdf'],struct('floating',true));
r = r.addRobotFromURDF([getDrakePath,'/examples/Quadrotor/quadrotor_fla.urdf'],[3;0;1],[0;0;0],struct('floating',true));
r = r.compile();
v = r.constructVisualizer();

Then you have an Atlas robot and a quadrotor in your visualizer.

[AndreasMegatranus]

Sounds good.

What is the state representation of r in this case? Is it positions
and velocities of the first robot, followed by positions and velocities of the second? Or is it
positions of the first and second, followed by velocities of the first and second?

Andreas

Quoting Hongkai Dai notifications@github.com:

Yes, you can put two robots into r, by calling addRobotFromURDF,
addRobotFromSDF or addRobotFromURDFString. One example is

r =  
RigidBodyManipulator([getDrakePath,'/examples/Atlas/urdf/atlas_minimal_contact.urdf'],struct('floating',true));
r =  
r.addRobotFromURDF([getDrakePath,'/examples/Quadrotor/quadrotor_fla.urdf'],[3;0;1],[0;0;0],struct('floating',true));
r = r.compile();
v = r.constructVisualizer();

Then you have an Atlas robot and a quadrotor in your visualizer.

---

You are receiving this because you authored the thread. Reply to this email directly or view it on GitHub: https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-211392843

[hongkai-dai]

The order is [position1;position2;velocity1;velocity2]. We have a function getStateFrame() that will show you the frame of the state. If you do

x_init = r.getInitialState();
Point(r.getStateFrame(),x_init)

You will see how the states are ordered.

[AndreasMegatranus]

Hi,

I tried

r =
RigidBodyManipulator([getDrakePath,'/examples/Atlas/urdf/atlas_minimal_contact.urdf'],struct('floating',true));

r.addRobotFromURDF([getDrakePath,'/examples/Atlas/urdf/atlas_minimal_contact.urdf'],struct('floating',true));

to try to get two Atlas robots. This resulted in the following error:

Error using horzcat The following error occurred converting from double to struct: Conversion to struct from double is not possible.

Error in RigidBodyManipulator/addJoint (line 522) child.Ttree = [rpy2rotmat(rpy), xyz; 0,0,0,1];

Error in RigidBodyManipulator/addRobotFromURDF>parseRobot (line 153) model = addJoint(model,'weld','fixed',weldLink,rootlink(i),xyz,rpy);

Error in RigidBodyManipulator/addRobotFromURDF (line 55) model = parseRobot(model,robot,xyz,rpy,options);

Error in runAtlasManip4 (line 43) r.addRobotFromURDF([getDrakePath,'/examples/Atlas/urdf/atlas_minimal_contact.urdf'],struct('floating',true));

Quoting Hongkai Dai notifications@github.com:

Yes, you can put two robots into r, by calling addRobotFromURDF,
addRobotFromSDF or addRobotFromURDFString. One example is

r =  
RigidBodyManipulator([getDrakePath,'/examples/Atlas/urdf/atlas_minimal_contact.urdf'],struct('floating',true));
r =  
r.addRobotFromURDF([getDrakePath,'/examples/Quadrotor/quadrotor_fla.urdf'],[3;0;1],[0;0;0],struct('floating',true));
r = r.compile();
v = r.constructVisualizer();

Then you have an Atlas robot and a quadrotor in your visualizer.

---

You are receiving this because you authored the thread. Reply to this email directly or view it on GitHub: https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-211392843

[hongkai-dai]

when calling addRobotFromURDF, the second argument should be the xyz position, the third is rpy for roll pitch and yaw. So if you do

r=RigidBodyManipulator([getDrakePath,'/examples/Atlas/urdf/atlas_minimal_contact.urdf'],struct('floating',true));

r=r.addRobotFromURDF([getDrakePath,'/examples/Atlas/urdf/atlas_minimal_contact.urdf'],XYZ_POS,RPY,struct('floating',true));

with some given values on XYZ_POS and RPY, it should work.

[AndreasMegatranus]

That helped. However, when I subsequently do r.getInitialState(), I
get a vector of 72 elements, rather than the 144 I would expect for two Atlas robots.

Quoting Hongkai Dai notifications@github.com:

when calling addRobotFromURDF, the second argument should be the
xyz position, the third is rpy for roll pitch and yaw. So if you
do

r=RigidBodyManipulator([getDrakePath,'/examples/Atlas/urdf/atlas_minimal_contact.urdf'],struct('floating',true));

r=r.addRobotFromURDF([getDrakePath,'/examples/Atlas/urdf/atlas_minimal_contact.urdf'],XYZ_POS,RPY,struct('floating',true));

with some given values on XYZ_POS and RPY, it should work.

---

You are receiving this because you authored the thread. Reply to this email directly or view it on GitHub: https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-216718002

[liangfok]

Maybe there is a model name clash when trying to add two models with the same name.

[hongkai-dai]

I do not think the model name should cause a problem. @AndreasMegatranus , did you return r as a return argument when you call addRobotFromURDF? This is what I did

r = RigidBodyManipulator([getDrakePath,'/examples/Atlas/urdf/atlas_minimal_contact.urdf'],struct('floating',true));
r = r.addRobotFromURDF([getDrakePath,'/examples/Atlas/urdf/atlas_minimal_contact.urdf'],[],[],struct('floating',true));

Notice that on the second line, r is returned on the left of the assignment. MATLAB syntax requires returning the object, if you want to make modification on the object by calling its method.

In most cases, after you add models to a RigidBodyManipulator, you will need to compile the robot, by calling the compile function, in order to call certain subsequent functions. So I would add the robot like this

r = RigidBodyManipulator([getDrakePath,'/examples/Atlas/urdf/atlas_minimal_contact.urdf'],struct('floating',true));
r = r.addRobotFromURDF([getDrakePath,'/examples/Atlas/urdf/atlas_minimal_contact.urdf'],[],[],struct('floating',true));
r = r.compile();

Then when I call

x0 = r.getInitialState()

I get a 144 x 1 vector.

Let me know if this helps.

[AndreasMegatranus]

Yes, assigning r fixed this. I should have realized this myself! I'm
aware that MATLAB syntax requires returning the object to achieve the modification. Thank you for the
prompt help!

Andreas

Quoting Hongkai Dai notifications@github.com:

I do not think the model name should cause a problem.
@AndreasMegatranus , did you return r as a return argument when
you call addRobotFromURDF? This is what I did

r =  
RigidBodyManipulator([getDrakePath,'/examples/Atlas/urdf/atlas_minimal_contact.urdf'],struct('floating',true));
r =  
r.addRobotFromURDF([getDrakePath,'/examples/Atlas/urdf/atlas_minimal_contact.urdf'],[],[],struct('floating',true));

Notice that on the second line, r is returned on the left of the
assignment. MATLAB syntax requires returning the object, if you want
to make modification on the object by calling its method.

In most cases, after you add models to a RigidBodyManipulator, you
will need to compile the robot, by calling the compile function,
in order to call certain subsequent functions. So I would add the
robot like this

r =  
RigidBodyManipulator([getDrakePath,'/examples/Atlas/urdf/atlas_minimal_contact.urdf'],struct('floating',true));
r =  
r.addRobotFromURDF([getDrakePath,'/examples/Atlas/urdf/atlas_minimal_contact.urdf'],[],[],struct('floating',true));
r = r.compile();

Then when I call

x0 = r.getInitialState()

I get a 144 x 1 vector.

Let me know if this helps.

---

You are receiving this because you were mentioned. Reply to this email directly or view it on GitHub: https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-216867286

[hongkai-dai]

Awesome, glad the problem is gone!

[AndreasMegatranus]

I am having a naming type of issue now.

The example code for runAtlasManip has a call

plan_settings =
QPLocomotionPlanSettings.fromQuasistaticQTraj(r,PPTrajectory(qtraj_pp),... struct('bodies_to_track',[pelvis, r_hand],... 'quat_task_to_world',repmat(uniformlyRandomQuat(),1,2),... 'translation_task_to_world',randn(3,2),... 'track_com_traj',true));

which requires pelvis and r_hand. These are obtained earlier by

pelvis = r.findLinkId('pelvis'); r_hand = r.findLinkId('r_hand');

These are failing now because 'pelvis' and 'r_hand' are no longer
unique. Should I be naming the robots somehow so that this doesn't happen? Can I omit
'bodies_to_track' in QPLocomotionPlanSettings?

Quoting Hongkai Dai notifications@github.com:

Awesome, glad the problem is gone!

---

You are receiving this because you were mentioned. Reply to this email directly or view it on GitHub: https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-216883769

[hongkai-dai]

I do not see a good way to get around this. In findLinkID function, we can specify which robot we are using, but I believe when we write QPLocomotionPlanSettings, our mental model is that there is just one single robot. (This code is used in DRC, in which you have only one Atlas). @AndreasMegatranus you could modify the code in QPLocomotionPlanSettings to specify which robot you are using. @RussTedrake @rdeits , do you know a better way to solve this?

[AndreasMegatranus]

I had a separate follow-up question. In the runAtlasManip example,
there is the following code:

 r_arm_idx = r.findPositionIndices('r_arm');
 plan_settings.untracked_joint_inds = r_arm_idx;

(this is part of the code that sets up manip_plan_data). As you know
from my previous emails, I had a lot of problems with stability; the arm seemed to not follow the
planned trajectories at all. So I tried commenting out this code, just as a guess, and magically,
everything worked better. Do you know what's going on here? What is the purpose of
untracked_joint_inds?

Quoting Hongkai Dai notifications@github.com:

I do not see a good way to get around this. In findLinkID
function, we can specify which robot we are using, but I believe
when we write QPLocomotionPlanSettings, our mental model is that
there is just one single robot. (This code is used in DRC, in which
you have only one Atlas). @AndreasMegatranus you could modify the
code in QPLocomotionPlanSettings to specify which robot you are
using. @RussTedrake @rdeits , do you know a better way to solve this?

---

You are receiving this because you were mentioned. Reply to this email directly or view it on GitHub: https://github.com/RobotLocomotion/drake/issues/2006#issuecomment-217027056

[hongkai-dai]

We can choose to track the end effector pose trajectory, or track the arm trajectory, or both. Previously it was just trying to track the end effector pose trajectory, with no tracking on the arm joints. Now by commenting out the untracked_joint_inds, it starts to track the right arm joints. Thanks for letting us know, that indicates some problem in the end effector tracking in our controller.

Sorry I did not have time to look into why the end effector tracking was bad. We are rewriting the backend controller in C++, and it will be a lot easier for us to debug the problem once that re-structure is completed.