Closed mohiz closed 9 years ago
Yes - there are a few descent tools for that. What visualizer are you using?
On Jul 10, 2015, at 11:00 AM, MohsenTamiz notifications@github.com wrote:
My friends and I are working on drake for some months. We could get to some experience on NAO robot by drake, Now we faced with a problem getting information of coordinate frames. This is our scenario, we have a robot plant and we have connected a pd control to it, now we want to find a way to visualize torque applied to the robot. Since we just have simulate outputs which are trajectories of the OutputFrame and the StateFrame we have not a direct way to get the torque applied to the robot. There are some ways to get around this trouble but all of them seem tricky and not work as well as we expect. For example we add a prallel system to the robot that just captures the input of the robot but this system could not properly use with pdcontrol function. As another way we cascade it with a unit DrakeSystem that was saving the input variables(as sensor classes works, we guess), but it seems not to be the correct way. In general, is it possible to have some system in drake which works as a scope in simulink, and can we get an arbitrary frame of a DynamicalSystem to visulize it after simulation?
Thanks in advance
— Reply to this email directly or view it on GitHub.
We have a biped robot that can walk stably on a terrain and we can watch its walking using a class that constructs a rigid body visualizer.
This is a part of our code:
P = 10*ones(1,length(nao_visualizer.planar_lip.actuator));
D = ones(1,length(nao_visualizer.planar_lip.actuator));
Kp = diag(P);
Kd = diag(D);
pd_control = pdcontrol(nao_biped,Kp,Kd);
c = DesiredTra(pd_control);
sys = cascade(c,pd_control);
where:
After running simulate by using command like this:
[y, x] = simulate(sys, [0 2.5], [c.q_initial;zeros(6,1)]);
We can visualize a walking robot using our visualizer and see also states of the robot using a ScopeVisualizer. But we need to see the torque that generated by pd_control, it is the input frame of the nao_biped object but it seems there is no trajectory assigned to it after simulation.
That all sounds great.
There are a few tools for that. They all revolve around the basic idea of publishing LCM messages from inside the thing you are debugging.
util/scope
command inside your function, and then you start a separate matlab process and run util/lcmScope
. If that looks like your preferred solution, i can say more.Both of these will spew lcm messages at simulation time. In order to have those lcm messages at playback time, the simulate command takes in an option capture_lcm_channels
. And the visualizer playback
method takes an optional lcmlog
. See https://github.com/RobotLocomotion/drake/blob/master/examples/Quadrotor/Quadrotor.m in the method runOpenLoop
for an example.
For what it's worth, we do also have a standalone scope program that we use here written by @patmarion . It's on our list of things to open-source (it just takes time). The lcm-spy
in the newest versions of lcm also has some nice plotting capabilities (thanks to @andybarry) which would plot any lcm messages of your choosing.
In this case, I think the visualization isn't the key issue -- it's getting the numbers out. One thing I've done is to use the state tape that comes out of simulation and push that through my controller, which gives the the control inputs. It's not particularly pretty, but it gives you utraj
which is what I think you're looking for.
Russ -- are there easier way to get utraj
out of a simulation?
@RussTedrake After running the example you mentioned using Quadrotor.runOpenLoop I get this error:
Error using Trajectory/inFrame (line 69)
couldn't find a coordinate transform from the trajectory frame quadrotorPosition+quadrotorVelocity to the requested frame
lidar+quadrotorPosition+quadrotorVelocity
Error in Trajectory/subsref (line 216)
[varargout{:}] = builtin('subsref',a,s);
Error in Visualizer/playback (line 85)
xtraj = xtraj.inFrame(obj.getInputFrame); % try to convert it
Error in Quadrotor.runOpenLoop (line 133)
v.playback(xtraj,struct('lcmlog',lcmlog));
But we searched further on this topic, I guess that lcmgl is not the proper choice for us because we don't need to visualize the data with a certain shape, we just want to represent these data like what ScopeVisualizer does. After some searches I found the runLCM method of DrakeSystem and some tests like testLCMPlant of Pendulum example, I also checked two functions of CoordinateFrame, setupLcmInputs and setupLcmOutputs that I think they construct lcm messages of coordinate frames. Finally, I think we must construct lcm messages using the above methods and capture them using capture_lcm_channel or util/scope. Is there any example or documentation that makes these steps more clear?
Sure. If you use the mimoFeedback and mimoCascade functions, you can arrange for whatever inputs/outputs you want for the combined system.
On Jul 12, 2015, at 1:40 PM, Andy Barry notifications@github.com wrote:
In this case, I think the visualization isn't the key issue -- it's getting the numbers out. One thing I've done is to use the state tape that comes out of simulation and push that through my controller, which gives the the control inputs. It's not particularly pretty, but it gives you utraj which is what I think you're looking for.
Russ -- are there easier way to get utraj out of a simulation?
— Reply to this email directly or view it on GitHub.
It is not a general solution; it makes the system larger and larger just for capturing some data, and I think it is a common task to get arbitrary data during simulation or playback for further analysis. I think a more general solution makes this task more flexible and easier in a large scale problem.
On Jul 12, 2015 10:10 PM, "Andy Barry" notifications@github.com wrote:
In this case, I think the visualization isn't the key issue -- it's getting the numbers out. One thing I've done is to use the state tape that comes out of simulation and push that through my controller, which gives the the control inputs. It's not particularly pretty, but it gives you utraj which is what I think you're looking for.
Russ -- are there easier way to get utraj out of a simulation?
— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1153#issuecomment-120749537 .
Checking in on this. Does the mimoFeedback / mimoCascade solution work for you in the short term?
Otherwise, for documentation on how approach the LCM version, you should start here: http://lcm-proj.github.io/
Yes, I could do this by code something like this:
[pdff, pdfb]= pdcontrol(nao_biped,Kp,Kd);
unit_drake = LinearSystem([], [], [], [], [], [eye(nao_biped.getInputFrame().dim) eye(nao_biped.getInputFrame().dim); eye(nao_biped.getInputFrame().dim) eye(nao_biped.getInputFrame().dim)]);
unit_drake = unit_drake.setInputFrame(MultiCoordinateFrame({nao_biped.getInputFrame(), nao_biped.getInputFrame()}));
multi_coordinate = MultiCoordinateFrame({nao_biped.getInputFrame(), nao_biped.getInputFrame()});
unit_drake = unit_drake.setOutputFrame(multi_coordinate);
nao_biped = nao_biped.setInputFrame(unit_drake.getOutputFrame().frame{2});
connection(1).from_output = 2;
connection(1).to_input = 1;
sys_cascade = mimoCascade(unit_drake, nao_biped, connection);
pdfb_sys_cascade_connection(1).from_output = 1;
pdfb_sys_cascade_connection(1).to_input = 2;
sys_cascade_pdfb_connection(1).from_output = 2;
sys_cascade_pdfb_connection(1).to_input = 1;
sys_cascade_pdfb_connection(2).from_output = 3;
sys_cascade_pdfb_connection(2).to_input = 2;
output_select(1).system = 2;
output_select(1).output = 1;
output_select(2).system = 2;
output_select(2).output = 2;
output_select(3).system = 2;
output_select(3).output = 3;
sys_feedback = mimoFeedback(pdfb, sys_cascade, pdfb_sys_cascade_connection, sys_cascade_pdfb_connection, [], output_select);
sys = cascade(pdff, sys_feedback);
c = DesiredTra(pdff, nao_biped);
sys = cascade(c, sys);
After this process I have a system that has all coordinate frames which I have expected. But now I am working on separating the output trajectory to two sub trajectories to able to visualize each part depend on its type. But now I have a problem using subsref, I used this code:
% after running simulation i have y which has three coordinate frame,
%first is utraj, second is position of joints and third is velocity of joints,
%for visualizing robot we just need position of joints
s.type = '()';
s.subs = {7:12};
y_nao = subsref(y, s);
nao_visualizer = nao_visualizer.setInputFrame(y_nao.getOutputFrame());
nao_visualizer.playback(y_nao, struct('slider',true));
This code starts slider but play button does not work and always prints this error:
Error while evaluating TimerFcn for timer 'timer-1'
Index exceeds matrix dimensions.
What is my mistake to get sub trajectories of the main trajectory?
The output frame of your new system will be a MultiCoordinateFrame
. You can use the splitCoordinates
method to break it apart, so you don't have to do your own subsref.
>> help MultiCoordinateFrame/splitCoordinates
Extract values of individual CoordinateFrames from a vector in the
multi-frame.
@param vector_vals a vector in the multi-frame
Usage: cv = splitCoordinates(obj,vv);
a = cv{1}; b=cv{2};
or
[a,b] = splitCoordinates(obj,vv)
On Jul 16, 2015, at 8:26 AM, MohsenTamiz notifications@github.com wrote:
Yes, I could do this by code something like this:
[pdff, pdfb]= pdcontrol(nao_biped,Kp,Kd);
unit_drake = LinearSystem([], [], [], [], [], [eye(nao_biped.getInputFrame().dim) eye(nao_biped.getInputFrame().dim); eye(nao_biped.getInputFrame().dim) eye(nao_biped.getInputFrame().dim)]); unit_drake = unit_drake.setInputFrame(MultiCoordinateFrame({nao_biped.getInputFrame(), nao_biped.getInputFrame()})); multi_coordinate = MultiCoordinateFrame({nao_biped.getInputFrame(), nao_biped.getInputFrame()}); unit_drake = unit_drake.setOutputFrame(multi_coordinate); nao_biped = nao_biped.setInputFrame(unit_drake.getOutputFrame().frame{2});
connection(1).from_output = 2; connection(1).to_input = 1;
sys_cascade = mimoCascade(unit_drake, nao_biped, connection);pdfb_sys_cascade_connection(1).from_output = 1; pdfb_sys_cascade_connection(1).to_input = 2;
sys_cascade_pdfb_connection(1).from_output = 2; sys_cascade_pdfb_connection(1).to_input = 1; sys_cascade_pdfb_connection(2).from_output = 3; sys_cascade_pdfb_connection(2).to_input = 2;
output_select(1).system = 2; output_select(1).output = 1; output_select(2).system = 2; output_select(2).output = 2; output_select(3).system = 2; output_select(3).output = 3;
sys_feedback = mimoFeedback(pdfb, sys_cascade, pdfb_sys_cascade_connection, sys_cascade_pdfb_connection, [], output_select);
sys = cascade(pdff, sys_feedback);
c = DesiredTra(pdff, nao_biped); sys = cascade(c, sys); After this process I have a system that has all coordinate frames which I have expected. But now I am working on separating the output trajectory to two sub trajectories to able to visualize each part depend on its type. But now I have a problem using subsref, I used this code:% after running simulation i have y which has three coordinate frame, %first is utraj, second is position of joints and third is velocity of joints, %for visualizing robot we just need position of joints s.type = '()'; s.subs = {7:12}; y_nao = subsref(y, s); nao_visualizer = nao_visualizer.setInputFrame(y_nao.getOutputFrame()); nao_visualizer.playback(y_nao, struct('slider',true)); This code starts slider but play button does not work and always prints this error:
Error while evaluating TimerFcn for timer 'timer-1'
Index exceeds matrix dimensions. What is my mistake to get sub trajectories of the main trajectory?
— Reply to this email directly or view it on GitHub.
I have a visualizer that expects a trajectory which is a sub trajectory of the output trajectory and its frame is one of the sub frame of the output frame. So, I decided to build a trajectory from the output trajectory that I can pass it to the visualizer. After some searches I do not think that splitCoordinates is exact method that I need. I did this in two separate ways, one using subsref that I mentioned before and another is a code like this:
% This is the same as subsref or a simple eval and subsref but i did it in the longer way
y_fcn = FunctionHandleTrajectory(@(t)(subsref(y_nao.getOutputFrame().splitCoordinates(y_nao.eval(t)),substruct('{}', {2}))), 6, y_nao.getBreaks())
y_fcn = y_fcn.setOutputTrajectory(nao_visualizer.getInputTrajectory)
Although both these ways work well on matlab command or another test functions, but they first print this warning:
Warning: creating function handle. this is potentially inefficient. consider implementing things a different way
> In FunctionHandleTrajectory>FunctionHandleTrajectory.FunctionHandleTrajectory at 13
And after running playback always I get this error:
Error while evaluating TimerFcn for timer 'timer-1'
Index exceeds matrix dimensions.
I think that FunctionHandleTrajectory was designed for scenarios like this. If I am wrong, is there another way to get sub trajectory from a main trajectory that can visualize it separately?
Yes, you can use a FunctionHandleTrajectory for this. Quiet the warning if you think it's fine using warning('off',...)
. I added the warning because FunctionHandleTrajectories tend to be infectious -- once you have one, if you do algebra on them (via operator overloads), then they go everywhere. So in the heavier use cases of passing trajectories through functions, etc, they can be painfully slow.
It's hard for me to be sure without a test I can run, but I think your splitCoordinates is incorrect. splitCoordinates returns two outputs in this case, not a single output which is a cell array. Can you call y_fcn.eval(t)
successfully from the command line (independent of the playback)?
On Jul 17, 2015, at 5:33 AM, MohsenTamiz notifications@github.com wrote:
I have a visualizer that expects a trajectory which is a sub trajectory of the output trajectory and its frame is one of the sub frame of the output frame. So, I decided to build a trajectory from the output trajectory that I can pass it to the visualizer. After some searches I do not think that splitCoordinates is exact method that I need. I did this in two separate ways, one using subsref that I mentioned before and another is a code like this:
% This is the same as subsref or a simple eval and subsref but i did it in the longer way y_fcn = FunctionHandleTrajectory(@(t)(subsref(y_nao.getOutputFrame().splitCoordinates(y_nao.eval(t)),substruct('{}', {2}))), 6, y_nao.getBreaks()) y_fcn = y_fcn.setOutputTrajectory(nao_visualizer.getInputTrajectory) Although both these ways work well on matlab command or another test functions, but they first print this warning:
Warning: creating function handle. this is potentially inefficient. consider implementing things a different way
In FunctionHandleTrajectory>FunctionHandleTrajectory.FunctionHandleTrajectory at 13 And after running playback always I get this error:
Error while evaluating TimerFcn for timer 'timer-1'
Index exceeds matrix dimensions. I think that FunctionHandleTrajectory was designed for scenarios like this. If I am wrong, is there another way to get sub trajectory from a main trajectory that can visualize it separately?
— Reply to this email directly or view it on GitHub.
Yes, I could. Not only in matlab command line but also I can pass it to a separate function and that function can call it correctly and generates the correct results. I did this work in 3 different ways, I mentioned both of them earlier and the last one is something like this:
y_fcn = FunctionHandleTrajectory(@(t)(subsref(y.eval(t), substruct('()',{7:12}), 6, y.getBreaks());
y_fcn = y_fcn.setOutputFrame(nao_visualizer.getInputFrame());
All of them behave same(correct in command line but raise the error inside playback), so I do not think that splitCoordinates generates the error. After running playback if I move the slider manually the robot moves on the visualizer figure but at the same time an error occur on matlab command line (and play button never works). If is it possible I can work on a smaller example that reproduce this error and send it to you or if It will be small enough I can write it down here?
That would be perfect
On Jul 17, 2015, at 8:23 AM, MohsenTamiz notifications@github.com wrote:
Yes, I could. Not only in matlab command line but also I can pass it to a separate function and that function can call it correctly and generates the correct results. I did this work in 3 different ways, I mentioned both of them earlier and the last one is something like this:
y_fcn = FunctionHandleTrajectory(@(t)(subsref(y.eval(t), substruct('()',{7:12}), 6, y.getBreaks()); y_fcn = y_fcn.setOutputFrame(nao_visualizer.getInputFrame()); All of them behave same(correct in command line but raise the error inside playback), so I do not think that splitCoordinates generates the error. After running playback if I move the slider manually the robot moves on the visualizer figure but at the same time an error occur on matlab command line (and play button never works). If is it possible I can work on a smaller example that reproduce this error and send it to you or if It will be small enough I can write it down here?
— Reply to this email directly or view it on GitHub.
As a colleague of @MohsenTamiz , I think the error rises because we have derived nao_biped
from TimeSteppingRigidBodyManipulator
class, this class gives us a discrete time system, so our trajectory is a DTTrajectory too and because of that we have a problem in y.eval(t)
,so sometimes y.eval(t)
returns an Empty matrix and playback doesn't work correctly.
We were supposed to send a simple example, I showed this issue by a simple pendulum example which you are able to use PlanarRigidBodyManipulator or TimeSteppingRigidBodyManipulator. By setting time_stepping=true
the error will occur.
function pdControl()
time_stepping = true;
if( time_stepping )
options = [];
options.twoD = 1;
p = TimeSteppingRigidBodyManipulator('Pendulum.urdf',0.001,options);
else
p = PlanarRigidBodyManipulator('Pendulum.urdf');
end
v = p.constructVisualizer();
Kp = 100;
Kd = 5;
[pdff, pdfb]= pdcontrol(p,Kp,Kd);
unit_drake = LinearSystem([], [], [], [], [], [eye(p.getInputFrame().dim) eye(p.getInputFrame().dim); eye(p.getInputFrame().dim) eye(p.getInputFrame().dim)]);
unit_drake = unit_drake.setInputFrame(MultiCoordinateFrame({p.getInputFrame(), p.getInputFrame()}));
multi_coordinate = MultiCoordinateFrame({p.getInputFrame(), p.getInputFrame()});
unit_drake = unit_drake.setOutputFrame(multi_coordinate);
p = p.setInputFrame(unit_drake.getOutputFrame().frame{2});
connection(1).from_output = 2;
connection(1).to_input = 1;
sys_cascade = mimoCascade(unit_drake, p, connection);
pdfb_sys_cascade_connection(1).from_output = 1;
pdfb_sys_cascade_connection(1).to_input = 1;
sys_cascade_pdfb_connection(1).from_output = 2;
sys_cascade_pdfb_connection(1).to_input = 1;
sys_cascade_pdfb_connection(2).from_output = 3;
sys_cascade_pdfb_connection(2).to_input = 2;
output_select(1).system = 2;
output_select(1).output = 1;
output_select(2).system = 2;
output_select(2).output = 2;
output_select(3).system = 2;
output_select(3).output = 3;
sys_feedback = mimoFeedback(pdfb, sys_cascade, pdfb_sys_cascade_connection, sys_cascade_pdfb_connection, [], output_select);
sys = cascade(pdff, sys_feedback);
c = ConstOrPassthroughSystem(1,1);
c = c.setOutputFrame(sys.getInputFrame());
sys = cascade(c, sys);
xtraj=simulate(sys,[0 4],[0;0]);
xtraj_robot = FunctionHandleTrajectory(@(t)(subsref(xtraj.eval(t),substruct('()',{2}))), 1, xtraj.getBreaks());
xtraj_robot = xtraj_robot.setOutputFrame(v.getInputFrame());
v.playback(xtraj_robot, struct('slider', true));
figure(1024);
utraj = FunctionHandleTrajectory(@(t)(subsref(xtraj.eval(t),substruct('()',{1}))), 1, xtraj.getBreaks());
utraj.fnplt();
end
Where Pendulum.urdf
is https://github.com/RobotLocomotion/drake/blob/master/examples/Pendulum/Pendulum.urdf
Sorry for the long post, I did not know a better way to send the codes.
Sorry that I haven’t had time to test this yet, but I’m pretty sure I have a (somewhat annoying) solution. The playback routines do try to be robust to having discrete time trajectories passed in. But you lose the fact that the trajectory is discrete when you wrap it in the FunctionHandleTrajectory — that’s another reason to avoid them.
If you do
xtraj_robot = setSampleTime(xtraj_robot,[.001;0]);
where the .001 is the sample time you passed into your timestepping method, then the playback method will only try to evaluate your function handle at those intervals.
Again, not tested. But hope it works (or is close).
On Jul 19, 2015, at 8:53 AM, Hosein Zabihi notifications@github.com wrote:
As a colleague of @MohsenTamiz https://github.com/MohsenTamiz , I think the error rises because we have derived noa_biped from TimeSteppingRigidBodyManipulator class, this class gives us a discrete time system so our trajectory is a DTTrajectory too and because of that we have a problem in y.eval(t),so sometimes y.eval(t) returns an Empty matrix. and playback doesn't work correctly. We were supposed to send a simple example, I showed this issue by a simple pendulum example which you are able to use PlanarRigidBodyManipulator or TimeSteppingRigidBodyManipulator. By setting time_stepping=true the error will occur.
function pdControl()
time_stepping = true; if( time_stepping ) options = []; options.twoD = 1; p = TimeSteppingRigidBodyManipulator('Pendulum.urdf',0.001,options); else p = PlanarRigidBodyManipulator('Pendulum.urdf'); end v = p.constructVisualizer(); Kp = 100; Kd = 5; [pdff, pdfb]= pdcontrol(p,Kp,Kd); unit_drake = LinearSystem([], [], [], [], [], [eye(p.getInputFrame().dim) eye(p.getInputFrame().dim); eye(p.getInputFrame().dim) eye(p.getInputFrame().dim)]); unit_drake = unit_drake.setInputFrame(MultiCoordinateFrame({p.getInputFrame(), p.getInputFrame()})); multi_coordinate = MultiCoordinateFrame({p.getInputFrame(), p.getInputFrame()}); unit_drake = unit_drake.setOutputFrame(multi_coordinate); p = p.setInputFrame(unit_drake.getOutputFrame().frame{2}); connection(1).from_output = 2; connection(1).to_input = 1; sys_cascade = mimoCascade(unit_drake, p, connection); pdfb_sys_cascade_connection(1).from_output = 1; pdfb_sys_cascade_connection(1).to_input = 1; sys_cascade_pdfb_connection(1).from_output = 2; sys_cascade_pdfb_connection(1).to_input = 1; sys_cascade_pdfb_connection(2).from_output = 3; sys_cascade_pdfb_connection(2).to_input = 2; output_select(1).system = 2; output_select(1).output = 1; output_select(2).system = 2; output_select(2).output = 2; output_select(3).system = 2; output_select(3).output = 3; sys_feedback = mimoFeedback(pdfb, sys_cascade, pdfb_sys_cascade_connection, sys_cascade_pdfb_connection, [], output_select); sys = cascade(pdff, sys_feedback); c = DesiredPoint(pdff); sys = cascade(c, sys); xtraj=simulate(sys,[0 4],[0;0]); xtraj_robot = FunctionHandleTrajectory(@(t)(subsref(xtraj.eval(t),substruct('()',{2}))), 1, xtraj.getBreaks()); xtraj_robot = xtraj_robot.setOutputFrame(v.getInputFrame()); v.playback(xtraj_robot, struct('slider', true)); figure(1024); utraj = FunctionHandleTrajectory(@(t)(subsref(xtraj.eval(t),substruct('()',{1}))), 1, xtraj.getBreaks()); utraj.fnplt();
end where Pendulum.urdf is https://github.com/RobotLocomotion/drake/blob/master/examples/Pendulum/Pendulum.urdf https://github.com/RobotLocomotion/drake/blob/master/examples/Pendulum/Pendulum.urdf sorry for the long post, I did not know a better way to send the codes.
— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1153#issuecomment-122659742.
Thank you for your time, it worked for us and everything is allright now, we maybe later take some times to work on lcm to make this task more general.
After writing example, we found also that we can simply call visualizer using the original output because drake can find transform from a MultiCoordiateFrame to its sub frames so it works simply by calling playback(May be it is added in newer commits, because we tested it after fetching the master code). But for using methods like fnplt()
we had to generate a FunctionHandleTrajectory as we did before.
Thank you again.
Glad to hear it
My friends and I are working on drake for some months. We could get to some experience on NAO robot by drake, Now we faced with a problem getting information of coordinate frames. This is our scenario, we have a robot plant and we have connected a pd control to it, now we want to find a way to visualize torque applied to the robot. Since we just have simulate outputs which are trajectories of the OutputFrame and the StateFrame we have not a direct way to get the torque applied to the robot. There are some ways to get around this trouble but all of them seem tricky and not work as well as we expect. For example we add a prallel system to the robot that just captures the input of the robot but this system could not properly use with pdcontrol function. As another way we cascade it with a unit DrakeSystem that was saving the input variables(as sensor classes works, we guess), but it seems not to be the correct way. In general, is it possible to have some system in drake which works as a scope in simulink, and can we get an arbitrary frame of a DynamicalSystem to visulize it after simulation?
Thanks in advance