search

RobotLocomotion / drake

Model-based design and verification for robotics.
https://drake.mit.edu
Other
3.33k stars 1.26k forks source link

setParams with RigidBodyManipulator? #1193

Closed aespielberg closed 9 years ago

aespielberg commented 9 years ago

Hi all,

I have a RigidBodyManipulator - simply, it is an Acrobot with parameters for the two link lengths (I can post it if it helps).

I am trying to do some stuff similar to what's happening in ParameterEstimation(). In particular, I want to set the parameters of my rigid body to be msspoly (or TrigPoly), and from that, calculate either the plant's dynamics using the dynamics() function. Thus, I try:

params_ = p.getParamFrame.getPoly

[   p1  ]
[   p2  ]

>> p_ = p.setParams(params_)
Error using msspoly/double (line 21)
Cannot cast non-constant msspoly to double.

Error in RigidBody/updateParams (line 319)
        body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430)
        model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053)
      model = applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p);

Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

RussTedrake commented 9 years ago

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

        body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

to 

    body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
    if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i});

(caveat — i didn’t test, but suspect it will work).

again, there are a number of places to change this. please send a pull request if it works!

On Jul 19, 2015, at 4:21 PM, aespielberg notifications@github.com wrote:

Hi all,

I have a RigidBodyManipulator - simply, it is an Acrobot with parameters for the two link lengths (I can post it if it helps).

I am trying to do some stuff similar to what's happening in ParameterEstimation(). In particular, I want to set the parameters of my rigid body to be msspoly (or TrigPoly), and from that, calculate either the plant's dynamics using the dynamics() function. Thus, I try:

params_ = p.getParamFrame.getPoly

[ p1 ] [ p2 ]

p = p.setParams(params) Error using msspoly/double (line 21) Cannot cast non-constant msspoly to double.

Error in RigidBody/updateParams (line 319) body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model = applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p); Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193.

aespielberg commented 9 years ago

Thanks! I will check this out tonight.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake notifications@github.com wrote:

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

to

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i});

(caveat — i didn’t test, but suspect it will work).

again, there are a number of places to change this. please send a pull request if it works!

  • Russ

On Jul 19, 2015, at 4:21 PM, aespielberg notifications@github.com wrote:

Hi all,

I have a RigidBodyManipulator - simply, it is an Acrobot with parameters for the two link lengths (I can post it if it helps).

I am trying to do some stuff similar to what's happening in ParameterEstimation(). In particular, I want to set the parameters of my rigid body to be msspoly (or TrigPoly), and from that, calculate either the plant's dynamics using the dynamics() function. Thus, I try:

params_ = p.getParamFrame.getPoly

[ p1 ] [ p2 ]

p = p.setParams(params) Error using msspoly/double (line 21) Cannot cast non-constant msspoly to double.

Error in RigidBody/updateParams (line 319) body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model = applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p); Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193>.

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430 .

RussTedrake commented 9 years ago

(to state the obvious, I missed the end of the if in my code below)

On Jul 20, 2015, at 5:14 PM, aespielberg notifications@github.com wrote:

Thanks! I will check this out tonight.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake notifications@github.com wrote:

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

to

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i});

(caveat — i didn’t test, but suspect it will work).

again, there are a number of places to change this. please send a pull request if it works!

  • Russ

On Jul 19, 2015, at 4:21 PM, aespielberg notifications@github.com wrote:

Hi all,

I have a RigidBodyManipulator - simply, it is an Acrobot with parameters for the two link lengths (I can post it if it helps).

I am trying to do some stuff similar to what's happening in ParameterEstimation(). In particular, I want to set the parameters of my rigid body to be msspoly (or TrigPoly), and from that, calculate either the plant's dynamics using the dynamics() function. Thus, I try:

params_ = p.getParamFrame.getPoly

[ p1 ] [ p2 ]

p = p.setParams(params) Error using msspoly/double (line 21) Cannot cast non-constant msspoly to double.

Error in RigidBody/updateParams (line 319) body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model = applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p); Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193>.

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430 .

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123041661.

aespielberg commented 9 years ago

This didn't work. Here are the changes I made:

line 319, the changes you said. line 35 of rigidbodyelement I made:

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
 if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i}); end

and line 73 of RigidBodyFrame I made:

obj.T = subs(obj.param_binding_T, poly, pval);
if (deg(obj.T) == 0) obj.T = double(obj.T); end

Then, I had an error which complained that isnan is not a member of msspoly in valuecheck.m at line 23. Thus, I inserted:

val = double(val);

on line 22.

Now, I am stuck with the following error:

p.setParams(param_frame.getPoly)
Error using constructModelmex
mxGetPr can only be called on arguments which correspond to Matlab doubles

Error in RigidBodyManipulator/createMexPointer (line 2076)
        obj.mex_model_ptr = constructModelmex(obj);

Error in RigidBodyManipulator/compile (line 807)
      model = createMexPointer(model);

Error in RigidBodyManipulator/setParams (line 1055)
      model = compile(model);

Assuming everything I did prior is kosher, I worry this means that something in the C++ mex files are not playing well with parameters. Any advice on how to resolve this?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:19 PM, Russ Tedrake notifications@github.com wrote:

(to state the obvious, I missed the end of the if in my code below)

On Jul 20, 2015, at 5:14 PM, aespielberg notifications@github.com wrote:

Thanks! I will check this out tonight.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake notifications@github.com wrote:

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

to

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i});

(caveat — i didn’t test, but suspect it will work).

again, there are a number of places to change this. please send a pull request if it works!

  • Russ

On Jul 19, 2015, at 4:21 PM, aespielberg notifications@github.com wrote:

Hi all,

I have a RigidBodyManipulator - simply, it is an Acrobot with parameters for the two link lengths (I can post it if it helps).

I am trying to do some stuff similar to what's happening in ParameterEstimation(). In particular, I want to set the parameters of my rigid body to be msspoly (or TrigPoly), and from that, calculate either the plant's dynamics using the dynamics() function. Thus, I try:

params_ = p.getParamFrame.getPoly

[ p1 ] [ p2 ]

p = p.setParams(params) Error using msspoly/double (line 21) Cannot cast non-constant msspoly to double.

Error in RigidBody/updateParams (line 319) body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model = applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p); Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193>.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123041661 .

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123043167 .

aespielberg commented 9 years ago

Hi all,

I am hoping to spend my weekend getting analytical, parameterized gradients of my RigidBody. I'd like to help fixing the functionality of RigidBody as much as I can here, but I'm not sure where to start. Would someone be able to provide me a high-level overview of how msspoly's are handled in the non-RigidBody case, and how the RigidBody case is different? Is it that the standard Manipulator is abstract and therefore parameters are just overriding user-defined variables, and the user is then responsible for how to use them, and that the rigidbody tries to do this with links and joints, so it intends to implement as much of this functionality automatically? What role do the C++ files have in all of this implementation?

-Andy S.

On Mon, Jul 20, 2015 at 10:08 PM, Andrew Spielberg aespielberg@gmail.com wrote:

This didn't work. Here are the changes I made:

line 319, the changes you said. line 35 of rigidbodyelement I made:

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
 if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i}); end

and line 73 of RigidBodyFrame I made:

obj.T = subs(obj.param_binding_T, poly, pval);
if (deg(obj.T) == 0) obj.T = double(obj.T); end

Then, I had an error which complained that isnan is not a member of msspoly in valuecheck.m at line 23. Thus, I inserted:

val = double(val);

on line 22.

Now, I am stuck with the following error:

p.setParams(param_frame.getPoly)
Error using constructModelmex
mxGetPr can only be called on arguments which correspond to Matlab doubles

Error in RigidBodyManipulator/createMexPointer (line 2076)
        obj.mex_model_ptr = constructModelmex(obj);

Error in RigidBodyManipulator/compile (line 807)
      model = createMexPointer(model);

Error in RigidBodyManipulator/setParams (line 1055)
      model = compile(model);

Assuming everything I did prior is kosher, I worry this means that something in the C++ mex files are not playing well with parameters. Any advice on how to resolve this?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:19 PM, Russ Tedrake notifications@github.com wrote:

(to state the obvious, I missed the end of the if in my code below)

On Jul 20, 2015, at 5:14 PM, aespielberg notifications@github.com wrote:

Thanks! I will check this out tonight.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake <notifications@github.com

wrote:

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

to

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i});

(caveat — i didn’t test, but suspect it will work).

again, there are a number of places to change this. please send a pull request if it works!

  • Russ

On Jul 19, 2015, at 4:21 PM, aespielberg notifications@github.com wrote:

Hi all,

I have a RigidBodyManipulator - simply, it is an Acrobot with parameters for the two link lengths (I can post it if it helps).

I am trying to do some stuff similar to what's happening in ParameterEstimation(). In particular, I want to set the parameters of my rigid body to be msspoly (or TrigPoly), and from that, calculate either the plant's dynamics using the dynamics() function. Thus, I try:

params_ = p.getParamFrame.getPoly

[ p1 ] [ p2 ]

p = p.setParams(params) Error using msspoly/double (line 21) Cannot cast non-constant msspoly to double.

Error in RigidBody/updateParams (line 319) body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model = applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p); Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193>.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123041661 .

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123043167 .

RussTedrake commented 9 years ago

Short answer: ideally c++ files should have no impact on your for now (although your use case is potentially in danger if we make the cleanup that removes the code duplication between matlab and c++). if your t,x,or u are msspoly’s, then it calls the matlab version of kinematics/dynamics. if they are doubles, then it calls the c++ implementation.

Every system has to implement setParameters() in order to do what you do. The RigidBodyManipulator implements it by updating the parameters in the associated RigidBody class. Based on our previous thread, I think people have not tried setting the parameters themselves to be msspolys, but that absolutely should work.

On Jul 23, 2015, at 6:10 PM, aespielberg notifications@github.com wrote:

Hi all,

I am hoping to spend my weekend getting analytical, parameterized gradients of my RigidBody. I'd like to help fixing the functionality of RigidBody as much as I can here, but I'm not sure where to start. Would someone be able to provide me a high-level overview of how msspoly's are handled in the non-RigidBody case, and how the RigidBody case is different? Is it that the standard Manipulator is abstract and therefore parameters are just overriding user-defined variables, and the user is then responsible for how to use them, and that the rigidbody tries to do this with links and joints, so it intends to implement as much of this functionality automatically? What role do the C++ files have in all of this implementation?

-Andy S.

On Mon, Jul 20, 2015 at 10:08 PM, Andrew Spielberg aespielberg@gmail.com wrote:

This didn't work. Here are the changes I made:

line 319, the changes you said. line 35 of rigidbodyelement I made:

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i}); end

and line 73 of RigidBodyFrame I made:

obj.T = subs(obj.param_binding_T, poly, pval);
if (deg(obj.T) == 0) obj.T = double(obj.T); end

Then, I had an error which complained that isnan is not a member of msspoly in valuecheck.m at line 23. Thus, I inserted:

val = double(val);

on line 22.

Now, I am stuck with the following error:

p.setParams(param_frame.getPoly)
Error using constructModelmex
mxGetPr can only be called on arguments which correspond to Matlab doubles

Error in RigidBodyManipulator/createMexPointer (line 2076)
obj.mex_model_ptr = constructModelmex(obj);

Error in RigidBodyManipulator/compile (line 807)
model = createMexPointer(model);

Error in RigidBodyManipulator/setParams (line 1055)
model = compile(model);

Assuming everything I did prior is kosher, I worry this means that something in the C++ mex files are not playing well with parameters. Any advice on how to resolve this?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:19 PM, Russ Tedrake notifications@github.com wrote:

(to state the obvious, I missed the end of the if in my code below)

On Jul 20, 2015, at 5:14 PM, aespielberg notifications@github.com wrote:

Thanks! I will check this out tonight.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake <notifications@github.com

wrote:

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

to

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i});

(caveat — i didn’t test, but suspect it will work).

again, there are a number of places to change this. please send a pull request if it works!

  • Russ

On Jul 19, 2015, at 4:21 PM, aespielberg notifications@github.com wrote:

Hi all,

I have a RigidBodyManipulator - simply, it is an Acrobot with parameters for the two link lengths (I can post it if it helps).

I am trying to do some stuff similar to what's happening in ParameterEstimation(). In particular, I want to set the parameters of my rigid body to be msspoly (or TrigPoly), and from that, calculate either the plant's dynamics using the dynamics() function. Thus, I try:

params_ = p.getParamFrame.getPoly

[ p1 ] [ p2 ]

p = p.setParams(params) Error using msspoly/double (line 21) Cannot cast non-constant msspoly to double.

Error in RigidBody/updateParams (line 319) body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model = applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p); Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193>.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123041661 .

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123043167 .

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124252178.

aespielberg commented 9 years ago

Thanks for the info. So if I'm understanding correctly, mex files should never be getting called, so something is messing up before then.

I will look into where the bug is for this tonight. Hopefully I can get everything fixed up by tomorrow.

-Andy S.

On Thu, Jul 23, 2015 at 6:29 PM, Russ Tedrake notifications@github.com wrote:

Short answer: ideally c++ files should have no impact on your for now (although your use case is potentially in danger if we make the cleanup that removes the code duplication between matlab and c++). if your t,x,or u are msspoly’s, then it calls the matlab version of kinematics/dynamics. if they are doubles, then it calls the c++ implementation.

Every system has to implement setParameters() in order to do what you do. The RigidBodyManipulator implements it by updating the parameters in the associated RigidBody class. Based on our previous thread, I think people have not tried setting the parameters themselves to be msspolys, but that absolutely should work.

On Jul 23, 2015, at 6:10 PM, aespielberg notifications@github.com wrote:

Hi all,

I am hoping to spend my weekend getting analytical, parameterized gradients of my RigidBody. I'd like to help fixing the functionality of RigidBody as much as I can here, but I'm not sure where to start. Would someone be able to provide me a high-level overview of how msspoly's are handled in the non-RigidBody case, and how the RigidBody case is different? Is it that the standard Manipulator is abstract and therefore parameters are just overriding user-defined variables, and the user is then responsible for how to use them, and that the rigidbody tries to do this with links and joints, so it intends to implement as much of this functionality automatically? What role do the C++ files have in all of this implementation?

-Andy S.

On Mon, Jul 20, 2015 at 10:08 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

This didn't work. Here are the changes I made:

line 319, the changes you said. line 35 of rigidbodyelement I made:

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i}); end

and line 73 of RigidBodyFrame I made:

obj.T = subs(obj.param_binding_T, poly, pval);
if (deg(obj.T) == 0) obj.T = double(obj.T); end

Then, I had an error which complained that isnan is not a member of msspoly in valuecheck.m at line 23. Thus, I inserted:

val = double(val);

on line 22.

Now, I am stuck with the following error:

p.setParams(param_frame.getPoly)
Error using constructModelmex
mxGetPr can only be called on arguments which correspond to Matlab
doubles

Error in RigidBodyManipulator/createMexPointer (line 2076)
obj.mex_model_ptr = constructModelmex(obj);

Error in RigidBodyManipulator/compile (line 807)
model = createMexPointer(model);

Error in RigidBodyManipulator/setParams (line 1055)
model = compile(model);

Assuming everything I did prior is kosher, I worry this means that something in the C++ mex files are not playing well with parameters. Any advice on how to resolve this?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:19 PM, Russ Tedrake < notifications@github.com> wrote:

(to state the obvious, I missed the end of the if in my code below)

On Jul 20, 2015, at 5:14 PM, aespielberg notifications@github.com wrote:

Thanks! I will check this out tonight.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake < notifications@github.com

wrote:

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

body.(fn{i}) =
double(subs(body.param_bindings.(fn{i}),poly,pval));

to

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i});

(caveat — i didn’t test, but suspect it will work).

again, there are a number of places to change this. please send a pull request if it works!

  • Russ

On Jul 19, 2015, at 4:21 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I have a RigidBodyManipulator - simply, it is an Acrobot with parameters for the two link lengths (I can post it if it helps).

I am trying to do some stuff similar to what's happening in ParameterEstimation(). In particular, I want to set the parameters of my rigid body to be msspoly (or TrigPoly), and from that, calculate either the plant's dynamics using the dynamics() function. Thus, I try:

params_ = p.getParamFrame.getPoly

[ p1 ] [ p2 ]

p = p.setParams(params) Error using msspoly/double (line 21) Cannot cast non-constant msspoly to double.

Error in RigidBody/updateParams (line 319) body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model = applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p); Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193>.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123041661

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123043167

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124252178 .

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124255492 .

aespielberg commented 9 years ago

Quick question - what is the expected behavior if part of the system is msspoly, and part of it is double? For example, if mass, laddedmass, and T_body_to_joint are double and the rest of the system is msspoly?

-Andy S.

On Fri, Jul 24, 2015 at 12:21 PM, Andrew Spielberg aespielberg@gmail.com wrote:

Thanks for the info. So if I'm understanding correctly, mex files should never be getting called, so something is messing up before then.

I will look into where the bug is for this tonight. Hopefully I can get everything fixed up by tomorrow.

-Andy S.

On Thu, Jul 23, 2015 at 6:29 PM, Russ Tedrake notifications@github.com wrote:

Short answer: ideally c++ files should have no impact on your for now (although your use case is potentially in danger if we make the cleanup that removes the code duplication between matlab and c++). if your t,x,or u are msspoly’s, then it calls the matlab version of kinematics/dynamics. if they are doubles, then it calls the c++ implementation.

Every system has to implement setParameters() in order to do what you do. The RigidBodyManipulator implements it by updating the parameters in the associated RigidBody class. Based on our previous thread, I think people have not tried setting the parameters themselves to be msspolys, but that absolutely should work.

On Jul 23, 2015, at 6:10 PM, aespielberg notifications@github.com wrote:

Hi all,

I am hoping to spend my weekend getting analytical, parameterized gradients of my RigidBody. I'd like to help fixing the functionality of RigidBody as much as I can here, but I'm not sure where to start. Would someone be able to provide me a high-level overview of how msspoly's are handled in the non-RigidBody case, and how the RigidBody case is different? Is it that the standard Manipulator is abstract and therefore parameters are just overriding user-defined variables, and the user is then responsible for how to use them, and that the rigidbody tries to do this with links and joints, so it intends to implement as much of this functionality automatically? What role do the C++ files have in all of this implementation?

-Andy S.

On Mon, Jul 20, 2015 at 10:08 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

This didn't work. Here are the changes I made:

line 319, the changes you said. line 35 of rigidbodyelement I made:

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i}); end

and line 73 of RigidBodyFrame I made:

obj.T = subs(obj.param_binding_T, poly, pval);
if (deg(obj.T) == 0) obj.T = double(obj.T); end

Then, I had an error which complained that isnan is not a member of msspoly in valuecheck.m at line 23. Thus, I inserted:

val = double(val);

on line 22.

Now, I am stuck with the following error:

p.setParams(param_frame.getPoly)
Error using constructModelmex
mxGetPr can only be called on arguments which correspond to Matlab
doubles

Error in RigidBodyManipulator/createMexPointer (line 2076)
obj.mex_model_ptr = constructModelmex(obj);

Error in RigidBodyManipulator/compile (line 807)
model = createMexPointer(model);

Error in RigidBodyManipulator/setParams (line 1055)
model = compile(model);

Assuming everything I did prior is kosher, I worry this means that something in the C++ mex files are not playing well with parameters. Any advice on how to resolve this?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:19 PM, Russ Tedrake < notifications@github.com> wrote:

(to state the obvious, I missed the end of the if in my code below)

On Jul 20, 2015, at 5:14 PM, aespielberg <notifications@github.com

wrote:

Thanks! I will check this out tonight.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake < notifications@github.com

wrote:

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

body.(fn{i}) =
double(subs(body.param_bindings.(fn{i}),poly,pval));

to

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i});

(caveat — i didn’t test, but suspect it will work).

again, there are a number of places to change this. please send a pull request if it works!

  • Russ

On Jul 19, 2015, at 4:21 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I have a RigidBodyManipulator - simply, it is an Acrobot with parameters for the two link lengths (I can post it if it helps).

I am trying to do some stuff similar to what's happening in ParameterEstimation(). In particular, I want to set the parameters of my rigid body to be msspoly (or TrigPoly), and from that, calculate either the plant's dynamics using the dynamics() function. Thus, I try:

params_ = p.getParamFrame.getPoly

[ p1 ] [ p2 ]

p = p.setParams(params) Error using msspoly/double (line 21) Cannot cast non-constant msspoly to double.

Error in RigidBody/updateParams (line 319) body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model = applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p); Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193>.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123041661

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123043167

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124252178 .

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124255492 .

RussTedrake commented 9 years ago

That should be fine. We normally check the type of x (not the params) to decide whether to route to mex. many methods have an option use_mex that you could set to false if your x is a double. If you stay in matlab, then you should get msspoly's back.

On Jul 24, 2015, at 8:27 PM, aespielberg notifications@github.com wrote:

Quick question - what is the expected behavior if part of the system is msspoly, and part of it is double? For example, if mass, laddedmass, and T_body_to_joint are double and the rest of the system is msspoly?

-Andy S.

On Fri, Jul 24, 2015 at 12:21 PM, Andrew Spielberg aespielberg@gmail.com wrote:

Thanks for the info. So if I'm understanding correctly, mex files should never be getting called, so something is messing up before then.

I will look into where the bug is for this tonight. Hopefully I can get everything fixed up by tomorrow.

-Andy S.

On Thu, Jul 23, 2015 at 6:29 PM, Russ Tedrake notifications@github.com wrote:

Short answer: ideally c++ files should have no impact on your for now (although your use case is potentially in danger if we make the cleanup that removes the code duplication between matlab and c++). if your t,x,or u are msspoly’s, then it calls the matlab version of kinematics/dynamics. if they are doubles, then it calls the c++ implementation.

Every system has to implement setParameters() in order to do what you do. The RigidBodyManipulator implements it by updating the parameters in the associated RigidBody class. Based on our previous thread, I think people have not tried setting the parameters themselves to be msspolys, but that absolutely should work.

On Jul 23, 2015, at 6:10 PM, aespielberg notifications@github.com wrote:

Hi all,

I am hoping to spend my weekend getting analytical, parameterized gradients of my RigidBody. I'd like to help fixing the functionality of RigidBody as much as I can here, but I'm not sure where to start. Would someone be able to provide me a high-level overview of how msspoly's are handled in the non-RigidBody case, and how the RigidBody case is different? Is it that the standard Manipulator is abstract and therefore parameters are just overriding user-defined variables, and the user is then responsible for how to use them, and that the rigidbody tries to do this with links and joints, so it intends to implement as much of this functionality automatically? What role do the C++ files have in all of this implementation?

-Andy S.

On Mon, Jul 20, 2015 at 10:08 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

This didn't work. Here are the changes I made:

line 319, the changes you said. line 35 of rigidbodyelement I made:

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i}); end

and line 73 of RigidBodyFrame I made:

obj.T = subs(obj.param_binding_T, poly, pval);
if (deg(obj.T) == 0) obj.T = double(obj.T); end

Then, I had an error which complained that isnan is not a member of msspoly in valuecheck.m at line 23. Thus, I inserted:

val = double(val);

on line 22.

Now, I am stuck with the following error:

p.setParams(param_frame.getPoly)
Error using constructModelmex
mxGetPr can only be called on arguments which correspond to Matlab
doubles

Error in RigidBodyManipulator/createMexPointer (line 2076)
obj.mex_model_ptr = constructModelmex(obj);

Error in RigidBodyManipulator/compile (line 807)
model = createMexPointer(model);

Error in RigidBodyManipulator/setParams (line 1055)
model = compile(model);

Assuming everything I did prior is kosher, I worry this means that something in the C++ mex files are not playing well with parameters. Any advice on how to resolve this?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:19 PM, Russ Tedrake < notifications@github.com> wrote:

(to state the obvious, I missed the end of the if in my code below)

On Jul 20, 2015, at 5:14 PM, aespielberg <notifications@github.com

wrote:

Thanks! I will check this out tonight.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake < notifications@github.com

wrote:

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

body.(fn{i}) =
double(subs(body.param_bindings.(fn{i}),poly,pval));

to

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i});

(caveat — i didn’t test, but suspect it will work).

again, there are a number of places to change this. please send a pull request if it works!

  • Russ

On Jul 19, 2015, at 4:21 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I have a RigidBodyManipulator - simply, it is an Acrobot with parameters for the two link lengths (I can post it if it helps).

I am trying to do some stuff similar to what's happening in ParameterEstimation(). In particular, I want to set the parameters of my rigid body to be msspoly (or TrigPoly), and from that, calculate either the plant's dynamics using the dynamics() function. Thus, I try:

params_ = p.getParamFrame.getPoly

[ p1 ] [ p2 ]

p = p.setParams(params) Error using msspoly/double (line 21) Cannot cast non-constant msspoly to double.

Error in RigidBody/updateParams (line 319) body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model = applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p); Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193>.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123041661

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123043167

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124252178 .

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124255492 .

— Reply to this email directly or view it on GitHub.

aespielberg commented 9 years ago

I think I'm getting closer - follow-up: what happens if your plant is composed of several bodies, and some of them have doubles for, say, Xtree, and some of them have msspolys? It seems like that might be something that would break it, if it only looks at one of the bodies to determine what to do? Or is there a nested compilation procedure that handles each body individually?

Aside: I've been walking through the entire compilation procedure - one thing I"m not sure about is, what does constructParamFrame do? It returns a paramFrame with length1 and length2 but since I'm passing it new parameters which are msspoly [p1; p2], shouldn't it be p1 and p2?

-Andy S.

On Fri, Jul 24, 2015 at 9:32 PM, Russ Tedrake notifications@github.com wrote:

That should be fine. We normally check the type of x (not the params) to decide whether to route to mex. many methods have an option use_mex that you could set to false if your x is a double. If you stay in matlab, then you should get msspoly's back.

On Jul 24, 2015, at 8:27 PM, aespielberg notifications@github.com wrote:

Quick question - what is the expected behavior if part of the system is msspoly, and part of it is double? For example, if mass, laddedmass, and T_body_to_joint are double and the rest of the system is msspoly?

-Andy S.

On Fri, Jul 24, 2015 at 12:21 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Thanks for the info. So if I'm understanding correctly, mex files should never be getting called, so something is messing up before then.

I will look into where the bug is for this tonight. Hopefully I can get everything fixed up by tomorrow.

-Andy S.

On Thu, Jul 23, 2015 at 6:29 PM, Russ Tedrake < notifications@github.com> wrote:

Short answer: ideally c++ files should have no impact on your for now (although your use case is potentially in danger if we make the cleanup that removes the code duplication between matlab and c++). if your t,x,or u are msspoly’s, then it calls the matlab version of kinematics/dynamics. if they are doubles, then it calls the c++ implementation.

Every system has to implement setParameters() in order to do what you do. The RigidBodyManipulator implements it by updating the parameters in the associated RigidBody class. Based on our previous thread, I think people have not tried setting the parameters themselves to be msspolys, but that absolutely should work.

On Jul 23, 2015, at 6:10 PM, aespielberg notifications@github.com wrote:

Hi all,

I am hoping to spend my weekend getting analytical, parameterized gradients of my RigidBody. I'd like to help fixing the functionality of RigidBody as much as I can here, but I'm not sure where to start. Would someone be able to provide me a high-level overview of how msspoly's are handled in the non-RigidBody case, and how the RigidBody case is different? Is it that the standard Manipulator is abstract and therefore parameters are just overriding user-defined variables, and the user is then responsible for how to use them, and that the rigidbody tries to do this with links and joints, so it intends to implement as much of this functionality automatically? What role do the C++ files have in all of this implementation?

-Andy S.

On Mon, Jul 20, 2015 at 10:08 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

This didn't work. Here are the changes I made:

line 319, the changes you said. line 35 of rigidbodyelement I made:

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i}); end

and line 73 of RigidBodyFrame I made:

obj.T = subs(obj.param_binding_T, poly, pval);
if (deg(obj.T) == 0) obj.T = double(obj.T); end

Then, I had an error which complained that isnan is not a member of msspoly in valuecheck.m at line 23. Thus, I inserted:

val = double(val);

on line 22.

Now, I am stuck with the following error:

p.setParams(param_frame.getPoly)
Error using constructModelmex
mxGetPr can only be called on arguments which correspond to Matlab
doubles

Error in RigidBodyManipulator/createMexPointer (line 2076)
obj.mex_model_ptr = constructModelmex(obj);

Error in RigidBodyManipulator/compile (line 807)
model = createMexPointer(model);

Error in RigidBodyManipulator/setParams (line 1055)
model = compile(model);

Assuming everything I did prior is kosher, I worry this means that something in the C++ mex files are not playing well with parameters. Any advice on how to resolve this?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:19 PM, Russ Tedrake < notifications@github.com> wrote:

(to state the obvious, I missed the end of the if in my code below)

On Jul 20, 2015, at 5:14 PM, aespielberg < notifications@github.com

wrote:

Thanks! I will check this out tonight.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake < notifications@github.com

wrote:

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

body.(fn{i}) =
double(subs(body.param_bindings.(fn{i}),poly,pval));

to

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i});

(caveat — i didn’t test, but suspect it will work).

again, there are a number of places to change this. please send a pull request if it works!

  • Russ

On Jul 19, 2015, at 4:21 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I have a RigidBodyManipulator - simply, it is an Acrobot with parameters for the two link lengths (I can post it if it helps).

I am trying to do some stuff similar to what's happening in ParameterEstimation(). In particular, I want to set the parameters of my rigid body to be msspoly (or TrigPoly), and from that, calculate either the plant's dynamics using the dynamics() function. Thus, I try:

params_ = p.getParamFrame.getPoly

[ p1 ] [ p2 ]

p = p.setParams(params) Error using msspoly/double (line 21) Cannot cast non-constant msspoly to double.

Error in RigidBody/updateParams (line 319) body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model = applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p); Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193>.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123041661

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123043167

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124252178

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124255492

.

— Reply to this email directly or view it on GitHub.

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124784295 .

aespielberg commented 9 years ago

One other problem: For now I decided to just comment out the offending mex line (dunno if that messes anything up but I decided to do that just so I could test out more things). However, when I then try to query the dynamics, I've run into this problem:

[xdot, dxdot] = p.dynamics(0, [pi; pi; 0.1; 0.1], [0.4])
Undefined operator '\' for input arguments of type 'msspoly'.

Error in RigidBodyManipulator/doKinematics>computeTransformGradients (line
340)
  T_body_to_parent = T{body.parent} \ T{i};

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273)
    kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
  kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
        [H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

msspoly can't use \, which is worrisome. Is there a canonical way to solve linear systems with msspoly's?

-Andy S.

On Fri, Jul 24, 2015 at 10:22 PM, Andrew Spielberg aespielberg@gmail.com wrote:

I think I'm getting closer - follow-up: what happens if your plant is composed of several bodies, and some of them have doubles for, say, Xtree, and some of them have msspolys? It seems like that might be something that would break it, if it only looks at one of the bodies to determine what to do? Or is there a nested compilation procedure that handles each body individually?

Aside: I've been walking through the entire compilation procedure - one thing I"m not sure about is, what does constructParamFrame do? It returns a paramFrame with length1 and length2 but since I'm passing it new parameters which are msspoly [p1; p2], shouldn't it be p1 and p2?

-Andy S.

On Fri, Jul 24, 2015 at 9:32 PM, Russ Tedrake notifications@github.com wrote:

That should be fine. We normally check the type of x (not the params) to decide whether to route to mex. many methods have an option use_mex that you could set to false if your x is a double. If you stay in matlab, then you should get msspoly's back.

On Jul 24, 2015, at 8:27 PM, aespielberg notifications@github.com wrote:

Quick question - what is the expected behavior if part of the system is msspoly, and part of it is double? For example, if mass, laddedmass, and T_body_to_joint are double and the rest of the system is msspoly?

-Andy S.

On Fri, Jul 24, 2015 at 12:21 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Thanks for the info. So if I'm understanding correctly, mex files should never be getting called, so something is messing up before then.

I will look into where the bug is for this tonight. Hopefully I can get everything fixed up by tomorrow.

-Andy S.

On Thu, Jul 23, 2015 at 6:29 PM, Russ Tedrake < notifications@github.com> wrote:

Short answer: ideally c++ files should have no impact on your for now (although your use case is potentially in danger if we make the cleanup that removes the code duplication between matlab and c++). if your t,x,or u are msspoly’s, then it calls the matlab version of kinematics/dynamics. if they are doubles, then it calls the c++ implementation.

Every system has to implement setParameters() in order to do what you do. The RigidBodyManipulator implements it by updating the parameters in the associated RigidBody class. Based on our previous thread, I think people have not tried setting the parameters themselves to be msspolys, but that absolutely should work.

On Jul 23, 2015, at 6:10 PM, aespielberg <notifications@github.com

wrote:

Hi all,

I am hoping to spend my weekend getting analytical, parameterized gradients of my RigidBody. I'd like to help fixing the functionality of RigidBody as much as I can here, but I'm not sure where to start. Would someone be able to provide me a high-level overview of how msspoly's are handled in the non-RigidBody case, and how the RigidBody case is different? Is it that the standard Manipulator is abstract and therefore parameters are just overriding user-defined variables, and the user is then responsible for how to use them, and that the rigidbody tries to do this with links and joints, so it intends to implement as much of this functionality automatically? What role do the C++ files have in all of this implementation?

-Andy S.

On Mon, Jul 20, 2015 at 10:08 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

This didn't work. Here are the changes I made:

line 319, the changes you said. line 35 of rigidbodyelement I made:

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i}); end

and line 73 of RigidBodyFrame I made:

obj.T = subs(obj.param_binding_T, poly, pval);
if (deg(obj.T) == 0) obj.T = double(obj.T); end

Then, I had an error which complained that isnan is not a member of msspoly in valuecheck.m at line 23. Thus, I inserted:

val = double(val);

on line 22.

Now, I am stuck with the following error:

p.setParams(param_frame.getPoly)
Error using constructModelmex
mxGetPr can only be called on arguments which correspond to
Matlab
doubles

Error in RigidBodyManipulator/createMexPointer (line 2076)
obj.mex_model_ptr = constructModelmex(obj);

Error in RigidBodyManipulator/compile (line 807)
model = createMexPointer(model);

Error in RigidBodyManipulator/setParams (line 1055)
model = compile(model);

Assuming everything I did prior is kosher, I worry this means that something in the C++ mex files are not playing well with parameters. Any advice on how to resolve this?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:19 PM, Russ Tedrake < notifications@github.com> wrote:

(to state the obvious, I missed the end of the if in my code below)

On Jul 20, 2015, at 5:14 PM, aespielberg < notifications@github.com

wrote:

Thanks! I will check this out tonight.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake < notifications@github.com

wrote:

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

body.(fn{i}) =
double(subs(body.param_bindings.(fn{i}),poly,pval));

to

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i});

(caveat — i didn’t test, but suspect it will work).

again, there are a number of places to change this. please send a pull request if it works!

  • Russ

On Jul 19, 2015, at 4:21 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I have a RigidBodyManipulator - simply, it is an Acrobot with parameters for the two link lengths (I can post it if it helps).

I am trying to do some stuff similar to what's happening in ParameterEstimation(). In particular, I want to set the parameters of my rigid body to be msspoly (or TrigPoly), and from that, calculate either the plant's dynamics using the dynamics() function. Thus, I try:

params_ = p.getParamFrame.getPoly

[ p1 ] [ p2 ]

p = p.setParams(params) Error using msspoly/double (line 21) Cannot cast non-constant msspoly to double.

Error in RigidBody/updateParams (line 319) body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model = applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p); Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193>.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123041661

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123043167

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124252178

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124255492

.

— Reply to this email directly or view it on GitHub.

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124784295 .

aespielberg commented 9 years ago

Sorry for so many comments in a row: right now I'm just making my "best possible fixes" even if they're not the right ones just to see what else breaks down the pipeline or if I can get a bandaged up version of this working.

On line 340 of doKinematics, I replaced:

T_body_to_parent = T{body.parent} \ T{i};

with:

T_body_to_parent = T{body.parent}^-1 * T{i};

Unfortunately now at line 345, I get:

Error using msspoly/subsasgn (line 80)
Subscripted assignment dimension mismatch.

Error in RigidBodyManipulator/doKinematics>computeTransformGradients (line
345)
  dT_body_to_parentdq(:, body.position_num) = dT_body_to_parentdqi;

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273)
    kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
  kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
        [H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

The RHS is 2x1 while the LHS is 16 x 1. I believe this is because as a double, numel returns 16, but on msspoly, it returns 1 (not sure why). I can write a hack to fix that though, so I'll try to do that for now. I am worried about how deep this rabbit hole goes, though.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 10:45 PM, Andrew Spielberg aespielberg@gmail.com wrote:

One other problem: For now I decided to just comment out the offending mex line (dunno if that messes anything up but I decided to do that just so I could test out more things). However, when I then try to query the dynamics, I've run into this problem:

[xdot, dxdot] = p.dynamics(0, [pi; pi; 0.1; 0.1], [0.4]) Undefined operator '\' for input arguments of type 'msspoly'.

Error in RigidBodyManipulator/doKinematics>computeTransformGradients (line 340) T_body_to_parent = T{body.parent} \ T{i};

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273) kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S, kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81) kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25) kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56) [H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

msspoly can't use \, which is worrisome. Is there a canonical way to solve linear systems with msspoly's?

-Andy S.

On Fri, Jul 24, 2015 at 10:22 PM, Andrew Spielberg aespielberg@gmail.com wrote:

I think I'm getting closer - follow-up: what happens if your plant is composed of several bodies, and some of them have doubles for, say, Xtree, and some of them have msspolys? It seems like that might be something that would break it, if it only looks at one of the bodies to determine what to do? Or is there a nested compilation procedure that handles each body individually?

Aside: I've been walking through the entire compilation procedure - one thing I"m not sure about is, what does constructParamFrame do? It returns a paramFrame with length1 and length2 but since I'm passing it new parameters which are msspoly [p1; p2], shouldn't it be p1 and p2?

-Andy S.

On Fri, Jul 24, 2015 at 9:32 PM, Russ Tedrake notifications@github.com wrote:

That should be fine. We normally check the type of x (not the params) to decide whether to route to mex. many methods have an option use_mex that you could set to false if your x is a double. If you stay in matlab, then you should get msspoly's back.

On Jul 24, 2015, at 8:27 PM, aespielberg notifications@github.com wrote:

Quick question - what is the expected behavior if part of the system is msspoly, and part of it is double? For example, if mass, laddedmass, and T_body_to_joint are double and the rest of the system is msspoly?

-Andy S.

On Fri, Jul 24, 2015 at 12:21 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Thanks for the info. So if I'm understanding correctly, mex files should never be getting called, so something is messing up before then.

I will look into where the bug is for this tonight. Hopefully I can get everything fixed up by tomorrow.

-Andy S.

On Thu, Jul 23, 2015 at 6:29 PM, Russ Tedrake < notifications@github.com> wrote:

Short answer: ideally c++ files should have no impact on your for now (although your use case is potentially in danger if we make the cleanup that removes the code duplication between matlab and c++). if your t,x,or u are msspoly’s, then it calls the matlab version of kinematics/dynamics. if they are doubles, then it calls the c++ implementation.

Every system has to implement setParameters() in order to do what you do. The RigidBodyManipulator implements it by updating the parameters in the associated RigidBody class. Based on our previous thread, I think people have not tried setting the parameters themselves to be msspolys, but that absolutely should work.

On Jul 23, 2015, at 6:10 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I am hoping to spend my weekend getting analytical, parameterized gradients of my RigidBody. I'd like to help fixing the functionality of RigidBody as much as I can here, but I'm not sure where to start. Would someone be able to provide me a high-level overview of how msspoly's are handled in the non-RigidBody case, and how the RigidBody case is different? Is it that the standard Manipulator is abstract and therefore parameters are just overriding user-defined variables, and the user is then responsible for how to use them, and that the rigidbody tries to do this with links and joints, so it intends to implement as much of this functionality automatically? What role do the C++ files have in all of this implementation?

-Andy S.

On Mon, Jul 20, 2015 at 10:08 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

This didn't work. Here are the changes I made:

line 319, the changes you said. line 35 of rigidbodyelement I made:

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i}); end

and line 73 of RigidBodyFrame I made:

obj.T = subs(obj.param_binding_T, poly, pval);
if (deg(obj.T) == 0) obj.T = double(obj.T); end

Then, I had an error which complained that isnan is not a member of msspoly in valuecheck.m at line 23. Thus, I inserted:

val = double(val);

on line 22.

Now, I am stuck with the following error:

p.setParams(param_frame.getPoly)
Error using constructModelmex
mxGetPr can only be called on arguments which correspond to
Matlab
doubles

Error in RigidBodyManipulator/createMexPointer (line 2076)
obj.mex_model_ptr = constructModelmex(obj);

Error in RigidBodyManipulator/compile (line 807)
model = createMexPointer(model);

Error in RigidBodyManipulator/setParams (line 1055)
model = compile(model);

Assuming everything I did prior is kosher, I worry this means that something in the C++ mex files are not playing well with parameters. Any advice on how to resolve this?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:19 PM, Russ Tedrake < notifications@github.com> wrote:

(to state the obvious, I missed the end of the if in my code below)

On Jul 20, 2015, at 5:14 PM, aespielberg < notifications@github.com

wrote:

Thanks! I will check this out tonight.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake < notifications@github.com

wrote:

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

body.(fn{i}) =
double(subs(body.param_bindings.(fn{i}),poly,pval));

to

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) =
double(body.fn{i});

(caveat — i didn’t test, but suspect it will work).

again, there are a number of places to change this. please send a pull request if it works!

  • Russ

On Jul 19, 2015, at 4:21 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I have a RigidBodyManipulator - simply, it is an Acrobot with parameters for the two link lengths (I can post it if it helps).

I am trying to do some stuff similar to what's happening in ParameterEstimation(). In particular, I want to set the parameters of my rigid body to be msspoly (or TrigPoly), and from that, calculate either the plant's dynamics using the dynamics() function. Thus, I try:

params_ = p.getParamFrame.getPoly

[ p1 ] [ p2 ]

p = p.setParams(params) Error using msspoly/double (line 21) Cannot cast non-constant msspoly to double.

Error in RigidBody/updateParams (line 319) body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model = applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p); Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193>.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123041661

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123043167

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124252178

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124255492

.

— Reply to this email directly or view it on GitHub.

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124784295 .

aespielberg commented 9 years ago

(Trying to figure out if replacing the numel with: length(T{i}(:)), nq) will work, but dHomogTrans I now realize is returning a 15 x 1 vector when computed with a 4x4 msspoly as opposed to a 4 x 4 double, and I'm not sure why.)

-Andy S.

On Fri, Jul 24, 2015 at 11:00 PM, Andrew Spielberg aespielberg@gmail.com wrote:

Sorry for so many comments in a row: right now I'm just making my "best possible fixes" even if they're not the right ones just to see what else breaks down the pipeline or if I can get a bandaged up version of this working.

On line 340 of doKinematics, I replaced:

T_body_to_parent = T{body.parent} \ T{i};

with:

T_body_to_parent = T{body.parent}^-1 * T{i};

Unfortunately now at line 345, I get:

Error using msspoly/subsasgn (line 80)
Subscripted assignment dimension mismatch.

Error in RigidBodyManipulator/doKinematics>computeTransformGradients (line
345)
  dT_body_to_parentdq(:, body.position_num) = dT_body_to_parentdqi;

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273)
    kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
  kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
        [H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

The RHS is 2x1 while the LHS is 16 x 1. I believe this is because as a double, numel returns 16, but on msspoly, it returns 1 (not sure why). I can write a hack to fix that though, so I'll try to do that for now. I am worried about how deep this rabbit hole goes, though.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 10:45 PM, Andrew Spielberg aespielberg@gmail.com wrote:

One other problem: For now I decided to just comment out the offending mex line (dunno if that messes anything up but I decided to do that just so I could test out more things). However, when I then try to query the dynamics, I've run into this problem:

[xdot, dxdot] = p.dynamics(0, [pi; pi; 0.1; 0.1], [0.4]) Undefined operator '\' for input arguments of type 'msspoly'.

Error in RigidBodyManipulator/doKinematics>computeTransformGradients (line 340) T_body_to_parent = T{body.parent} \ T{i};

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273) kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S, kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81) kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25) kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56) [H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

msspoly can't use \, which is worrisome. Is there a canonical way to solve linear systems with msspoly's?

-Andy S.

On Fri, Jul 24, 2015 at 10:22 PM, Andrew Spielberg <aespielberg@gmail.com

wrote:

I think I'm getting closer - follow-up: what happens if your plant is composed of several bodies, and some of them have doubles for, say, Xtree, and some of them have msspolys? It seems like that might be something that would break it, if it only looks at one of the bodies to determine what to do? Or is there a nested compilation procedure that handles each body individually?

Aside: I've been walking through the entire compilation procedure - one thing I"m not sure about is, what does constructParamFrame do? It returns a paramFrame with length1 and length2 but since I'm passing it new parameters which are msspoly [p1; p2], shouldn't it be p1 and p2?

-Andy S.

On Fri, Jul 24, 2015 at 9:32 PM, Russ Tedrake notifications@github.com wrote:

That should be fine. We normally check the type of x (not the params) to decide whether to route to mex. many methods have an option use_mex that you could set to false if your x is a double. If you stay in matlab, then you should get msspoly's back.

On Jul 24, 2015, at 8:27 PM, aespielberg notifications@github.com wrote:

Quick question - what is the expected behavior if part of the system is msspoly, and part of it is double? For example, if mass, laddedmass, and T_body_to_joint are double and the rest of the system is msspoly?

-Andy S.

On Fri, Jul 24, 2015 at 12:21 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Thanks for the info. So if I'm understanding correctly, mex files should never be getting called, so something is messing up before then.

I will look into where the bug is for this tonight. Hopefully I can get everything fixed up by tomorrow.

-Andy S.

On Thu, Jul 23, 2015 at 6:29 PM, Russ Tedrake < notifications@github.com> wrote:

Short answer: ideally c++ files should have no impact on your for now (although your use case is potentially in danger if we make the cleanup that removes the code duplication between matlab and c++). if your t,x,or u are msspoly’s, then it calls the matlab version of kinematics/dynamics. if they are doubles, then it calls the c++ implementation.

Every system has to implement setParameters() in order to do what you do. The RigidBodyManipulator implements it by updating the parameters in the associated RigidBody class. Based on our previous thread, I think people have not tried setting the parameters themselves to be msspolys, but that absolutely should work.

On Jul 23, 2015, at 6:10 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I am hoping to spend my weekend getting analytical, parameterized gradients of my RigidBody. I'd like to help fixing the functionality of RigidBody as much as I can here, but I'm not sure where to start. Would someone be able to provide me a high-level overview of how msspoly's are handled in the non-RigidBody case, and how the RigidBody case is different? Is it that the standard Manipulator is abstract and therefore parameters are just overriding user-defined variables, and the user is then responsible for how to use them, and that the rigidbody tries to do this with links and joints, so it intends to implement as much of this functionality automatically? What role do the C++ files have in all of this implementation?

-Andy S.

On Mon, Jul 20, 2015 at 10:08 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

This didn't work. Here are the changes I made:

line 319, the changes you said. line 35 of rigidbodyelement I made:

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i});
end

and line 73 of RigidBodyFrame I made:

obj.T = subs(obj.param_binding_T, poly, pval);
if (deg(obj.T) == 0) obj.T = double(obj.T); end

Then, I had an error which complained that isnan is not a member of msspoly in valuecheck.m at line 23. Thus, I inserted:

val = double(val);

on line 22.

Now, I am stuck with the following error:

p.setParams(param_frame.getPoly)
Error using constructModelmex
mxGetPr can only be called on arguments which correspond to
Matlab
doubles

Error in RigidBodyManipulator/createMexPointer (line 2076)
obj.mex_model_ptr = constructModelmex(obj);

Error in RigidBodyManipulator/compile (line 807)
model = createMexPointer(model);

Error in RigidBodyManipulator/setParams (line 1055)
model = compile(model);

Assuming everything I did prior is kosher, I worry this means that something in the C++ mex files are not playing well with parameters. Any advice on how to resolve this?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:19 PM, Russ Tedrake < notifications@github.com> wrote:

(to state the obvious, I missed the end of the if in my code below)

On Jul 20, 2015, at 5:14 PM, aespielberg < notifications@github.com

wrote:

Thanks! I will check this out tonight.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake < notifications@github.com

wrote:

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

body.(fn{i}) =
double(subs(body.param_bindings.(fn{i}),poly,pval));

to

body.(fn{i}) =
subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) =
double(body.fn{i});

(caveat — i didn’t test, but suspect it will work).

again, there are a number of places to change this. please send a pull request if it works!

  • Russ

On Jul 19, 2015, at 4:21 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I have a RigidBodyManipulator - simply, it is an Acrobot with parameters for the two link lengths (I can post it if it helps).

I am trying to do some stuff similar to what's happening in ParameterEstimation(). In particular, I want to set the parameters of my rigid body to be msspoly (or TrigPoly), and from that, calculate either the plant's dynamics using the dynamics() function. Thus, I try:

params_ = p.getParamFrame.getPoly

[ p1 ] [ p2 ]

p = p.setParams(params) Error using msspoly/double (line 21) Cannot cast non-constant msspoly to double.

Error in RigidBody/updateParams (line 319) body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model =

applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p);

Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193>.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123041661

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123043167

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124252178

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124255492

.

— Reply to this email directly or view it on GitHub.

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124784295 .

aespielberg commented 9 years ago

I think adding ret(16, :) = 1; at the end of dHomogTrans fixes that problem, but I get a new error. Unfortunately I'm out of time for this evening and this problem looks a bit deeper than the others, so I have to leave it here for now. Maybe there are cleaner changes that can be made than all these things I've been doing? Sorry again for so many posts.

Error using msspoly/reshape (line 4)
Single argument must be 1-by-2.

Error in kron (line 35)
   B = reshape(B,[mb 1 nb 1]);

Error in matGradMultMat (line 22)
  B_diag = kron(eye(q),B);

Error in RigidBodyManipulator/doKinematics>computeTransformGradients (line
346)
  ret{i} = matGradMultMat(...

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273)
    kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
  kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
        [H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 11:14 PM, Andrew Spielberg aespielberg@gmail.com wrote:

(Trying to figure out if replacing the numel with: length(T{i}(:)), nq) will work, but dHomogTrans I now realize is returning a 15 x 1 vector when computed with a 4x4 msspoly as opposed to a 4 x 4 double, and I'm not sure why.)

-Andy S.

On Fri, Jul 24, 2015 at 11:00 PM, Andrew Spielberg aespielberg@gmail.com wrote:

Sorry for so many comments in a row: right now I'm just making my "best possible fixes" even if they're not the right ones just to see what else breaks down the pipeline or if I can get a bandaged up version of this working.

On line 340 of doKinematics, I replaced:

T_body_to_parent = T{body.parent} \ T{i};

with:

T_body_to_parent = T{body.parent}^-1 * T{i};

Unfortunately now at line 345, I get:

Error using msspoly/subsasgn (line 80)
Subscripted assignment dimension mismatch.

Error in RigidBodyManipulator/doKinematics>computeTransformGradients
(line 345)
  dT_body_to_parentdq(:, body.position_num) = dT_body_to_parentdqi;

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273)
    kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
  kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
        [H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

The RHS is 2x1 while the LHS is 16 x 1. I believe this is because as a double, numel returns 16, but on msspoly, it returns 1 (not sure why). I can write a hack to fix that though, so I'll try to do that for now. I am worried about how deep this rabbit hole goes, though.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 10:45 PM, Andrew Spielberg <aespielberg@gmail.com

wrote:

One other problem: For now I decided to just comment out the offending mex line (dunno if that messes anything up but I decided to do that just so I could test out more things). However, when I then try to query the dynamics, I've run into this problem:

[xdot, dxdot] = p.dynamics(0, [pi; pi; 0.1; 0.1], [0.4]) Undefined operator '\' for input arguments of type 'msspoly'.

Error in RigidBodyManipulator/doKinematics>computeTransformGradients (line 340) T_body_to_parent = T{body.parent} \ T{i};

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273) kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S, kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81) kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25) kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56) [H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

msspoly can't use \, which is worrisome. Is there a canonical way to solve linear systems with msspoly's?

-Andy S.

On Fri, Jul 24, 2015 at 10:22 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

I think I'm getting closer - follow-up: what happens if your plant is composed of several bodies, and some of them have doubles for, say, Xtree, and some of them have msspolys? It seems like that might be something that would break it, if it only looks at one of the bodies to determine what to do? Or is there a nested compilation procedure that handles each body individually?

Aside: I've been walking through the entire compilation procedure - one thing I"m not sure about is, what does constructParamFrame do? It returns a paramFrame with length1 and length2 but since I'm passing it new parameters which are msspoly [p1; p2], shouldn't it be p1 and p2?

-Andy S.

On Fri, Jul 24, 2015 at 9:32 PM, Russ Tedrake <notifications@github.com

wrote:

That should be fine. We normally check the type of x (not the params) to decide whether to route to mex. many methods have an option use_mex that you could set to false if your x is a double. If you stay in matlab, then you should get msspoly's back.

On Jul 24, 2015, at 8:27 PM, aespielberg notifications@github.com wrote:

Quick question - what is the expected behavior if part of the system is msspoly, and part of it is double? For example, if mass, laddedmass, and T_body_to_joint are double and the rest of the system is msspoly?

-Andy S.

On Fri, Jul 24, 2015 at 12:21 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Thanks for the info. So if I'm understanding correctly, mex files should never be getting called, so something is messing up before then.

I will look into where the bug is for this tonight. Hopefully I can get everything fixed up by tomorrow.

-Andy S.

On Thu, Jul 23, 2015 at 6:29 PM, Russ Tedrake < notifications@github.com> wrote:

Short answer: ideally c++ files should have no impact on your for now (although your use case is potentially in danger if we make the cleanup that removes the code duplication between matlab and c++). if your t,x,or u are msspoly’s, then it calls the matlab version of kinematics/dynamics. if they are doubles, then it calls the c++ implementation.

Every system has to implement setParameters() in order to do what you do. The RigidBodyManipulator implements it by updating the parameters in the associated RigidBody class. Based on our previous thread, I think people have not tried setting the parameters themselves to be msspolys, but that absolutely should work.

On Jul 23, 2015, at 6:10 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I am hoping to spend my weekend getting analytical, parameterized gradients of my RigidBody. I'd like to help fixing the functionality of RigidBody as much as I can here, but I'm not sure where to start. Would someone be able to provide me a high-level overview of how msspoly's are handled in the non-RigidBody case, and how the RigidBody case is different? Is it that the standard Manipulator is abstract and therefore parameters are just overriding user-defined variables, and the user is then responsible for how to use them, and that the rigidbody tries to do this with links and joints, so it intends to implement as much of this functionality automatically? What role do the C++ files have in all of this implementation?

-Andy S.

On Mon, Jul 20, 2015 at 10:08 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

This didn't work. Here are the changes I made:

line 319, the changes you said. line 35 of rigidbodyelement I made:

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i});
end

and line 73 of RigidBodyFrame I made:

obj.T = subs(obj.param_binding_T, poly, pval);
if (deg(obj.T) == 0) obj.T = double(obj.T); end

Then, I had an error which complained that isnan is not a member of msspoly in valuecheck.m at line 23. Thus, I inserted:

val = double(val);

on line 22.

Now, I am stuck with the following error:

p.setParams(param_frame.getPoly)
Error using constructModelmex
mxGetPr can only be called on arguments which correspond to
Matlab
doubles

Error in RigidBodyManipulator/createMexPointer (line 2076)
obj.mex_model_ptr = constructModelmex(obj);

Error in RigidBodyManipulator/compile (line 807)
model = createMexPointer(model);

Error in RigidBodyManipulator/setParams (line 1055)
model = compile(model);

Assuming everything I did prior is kosher, I worry this means that something in the C++ mex files are not playing well with parameters. Any advice on how to resolve this?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:19 PM, Russ Tedrake < notifications@github.com> wrote:

(to state the obvious, I missed the end of the if in my code below)

On Jul 20, 2015, at 5:14 PM, aespielberg < notifications@github.com

wrote:

Thanks! I will check this out tonight.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake < notifications@github.com

wrote:

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

body.(fn{i}) =
double(subs(body.param_bindings.(fn{i}),poly,pval));

to

body.(fn{i}) =
subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) =
double(body.fn{i});

(caveat — i didn’t test, but suspect it will work).

again, there are a number of places to change this. please send a pull request if it works!

  • Russ

On Jul 19, 2015, at 4:21 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I have a RigidBodyManipulator - simply, it is an Acrobot with parameters for the two link lengths (I can post it if it helps).

I am trying to do some stuff similar to what's happening in ParameterEstimation(). In particular, I want to set the parameters of my rigid body to be msspoly (or TrigPoly), and from that, calculate either the plant's dynamics using the dynamics() function. Thus, I try:

params_ = p.getParamFrame.getPoly

[ p1 ] [ p2 ]

p = p.setParams(params) Error using msspoly/double (line 21) Cannot cast non-constant msspoly to double.

Error in RigidBody/updateParams (line 319) body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model =

applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p);

Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193>.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123041661

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123043167

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124252178

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124255492

.

— Reply to this email directly or view it on GitHub.

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124784295 .

aespielberg commented 9 years ago

Looking into it now, this seems potentially bad - msspoly cannot be used with matlab's kronecker product because kron uses reshape with a 4 element array, and spotless is only happy with a 2 element array. I can (and probably will) code up my own version of the kronecker product, but it seems like there has to be a cleaner solution.

-Andy S.

On Fri, Jul 24, 2015 at 11:25 PM, Andrew Spielberg aespielberg@gmail.com wrote:

I think adding ret(16, :) = 1; at the end of dHomogTrans fixes that problem, but I get a new error. Unfortunately I'm out of time for this evening and this problem looks a bit deeper than the others, so I have to leave it here for now. Maybe there are cleaner changes that can be made than all these things I've been doing? Sorry again for so many posts.

Error using msspoly/reshape (line 4)
Single argument must be 1-by-2.

Error in kron (line 35)
   B = reshape(B,[mb 1 nb 1]);

Error in matGradMultMat (line 22)
  B_diag = kron(eye(q),B);

Error in RigidBodyManipulator/doKinematics>computeTransformGradients (line
346)
  ret{i} = matGradMultMat(...

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273)
    kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
  kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
        [H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 11:14 PM, Andrew Spielberg aespielberg@gmail.com wrote:

(Trying to figure out if replacing the numel with: length(T{i}(:)), nq) will work, but dHomogTrans I now realize is returning a 15 x 1 vector when computed with a 4x4 msspoly as opposed to a 4 x 4 double, and I'm not sure why.)

-Andy S.

On Fri, Jul 24, 2015 at 11:00 PM, Andrew Spielberg <aespielberg@gmail.com

wrote:

Sorry for so many comments in a row: right now I'm just making my "best possible fixes" even if they're not the right ones just to see what else breaks down the pipeline or if I can get a bandaged up version of this working.

On line 340 of doKinematics, I replaced:

T_body_to_parent = T{body.parent} \ T{i};

with:

T_body_to_parent = T{body.parent}^-1 * T{i};

Unfortunately now at line 345, I get:

Error using msspoly/subsasgn (line 80)
Subscripted assignment dimension mismatch.

Error in RigidBodyManipulator/doKinematics>computeTransformGradients
(line 345)
  dT_body_to_parentdq(:, body.position_num) = dT_body_to_parentdqi;

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273)
    kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
  kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
        [H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

The RHS is 2x1 while the LHS is 16 x 1. I believe this is because as a double, numel returns 16, but on msspoly, it returns 1 (not sure why). I can write a hack to fix that though, so I'll try to do that for now. I am worried about how deep this rabbit hole goes, though.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 10:45 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

One other problem: For now I decided to just comment out the offending mex line (dunno if that messes anything up but I decided to do that just so I could test out more things). However, when I then try to query the dynamics, I've run into this problem:

[xdot, dxdot] = p.dynamics(0, [pi; pi; 0.1; 0.1], [0.4]) Undefined operator '\' for input arguments of type 'msspoly'.

Error in RigidBodyManipulator/doKinematics>computeTransformGradients (line 340) T_body_to_parent = T{body.parent} \ T{i};

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273) kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S, kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81) kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25) kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56) [H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

msspoly can't use \, which is worrisome. Is there a canonical way to solve linear systems with msspoly's?

-Andy S.

On Fri, Jul 24, 2015 at 10:22 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

I think I'm getting closer - follow-up: what happens if your plant is composed of several bodies, and some of them have doubles for, say, Xtree, and some of them have msspolys? It seems like that might be something that would break it, if it only looks at one of the bodies to determine what to do? Or is there a nested compilation procedure that handles each body individually?

Aside: I've been walking through the entire compilation procedure - one thing I"m not sure about is, what does constructParamFrame do? It returns a paramFrame with length1 and length2 but since I'm passing it new parameters which are msspoly [p1; p2], shouldn't it be p1 and p2?

-Andy S.

On Fri, Jul 24, 2015 at 9:32 PM, Russ Tedrake < notifications@github.com> wrote:

That should be fine. We normally check the type of x (not the params) to decide whether to route to mex. many methods have an option use_mex that you could set to false if your x is a double. If you stay in matlab, then you should get msspoly's back.

On Jul 24, 2015, at 8:27 PM, aespielberg notifications@github.com wrote:

Quick question - what is the expected behavior if part of the system is msspoly, and part of it is double? For example, if mass, laddedmass, and T_body_to_joint are double and the rest of the system is msspoly?

-Andy S.

On Fri, Jul 24, 2015 at 12:21 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Thanks for the info. So if I'm understanding correctly, mex files should never be getting called, so something is messing up before then.

I will look into where the bug is for this tonight. Hopefully I can get everything fixed up by tomorrow.

-Andy S.

On Thu, Jul 23, 2015 at 6:29 PM, Russ Tedrake < notifications@github.com> wrote:

Short answer: ideally c++ files should have no impact on your for now (although your use case is potentially in danger if we make the cleanup that removes the code duplication between matlab and c++). if your t,x,or u are msspoly’s, then it calls the matlab version of kinematics/dynamics. if they are doubles, then it calls the c++ implementation.

Every system has to implement setParameters() in order to do what you do. The RigidBodyManipulator implements it by updating the parameters in the associated RigidBody class. Based on our previous thread, I think people have not tried setting the parameters themselves to be msspolys, but that absolutely should work.

On Jul 23, 2015, at 6:10 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I am hoping to spend my weekend getting analytical, parameterized gradients of my RigidBody. I'd like to help fixing the functionality of RigidBody as much as I can here, but I'm not sure where to start. Would someone be able to provide me a high-level overview of how msspoly's are handled in the non-RigidBody case, and how the RigidBody case is different? Is it that the standard Manipulator is abstract and therefore parameters are just overriding user-defined variables, and the user is then responsible for how to use them, and that the rigidbody tries to do this with links and joints, so it intends to implement as much of this functionality automatically? What role do the C++ files have in all of this implementation?

-Andy S.

On Mon, Jul 20, 2015 at 10:08 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

This didn't work. Here are the changes I made:

line 319, the changes you said. line 35 of rigidbodyelement I made:

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i});
end

and line 73 of RigidBodyFrame I made:

obj.T = subs(obj.param_binding_T, poly, pval);
if (deg(obj.T) == 0) obj.T = double(obj.T); end

Then, I had an error which complained that isnan is not a member of msspoly in valuecheck.m at line 23. Thus, I inserted:

val = double(val);

on line 22.

Now, I am stuck with the following error:

p.setParams(param_frame.getPoly)
Error using constructModelmex
mxGetPr can only be called on arguments which correspond to
Matlab
doubles

Error in RigidBodyManipulator/createMexPointer (line 2076)
obj.mex_model_ptr = constructModelmex(obj);

Error in RigidBodyManipulator/compile (line 807)
model = createMexPointer(model);

Error in RigidBodyManipulator/setParams (line 1055)
model = compile(model);

Assuming everything I did prior is kosher, I worry this means that something in the C++ mex files are not playing well with parameters. Any advice on how to resolve this?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:19 PM, Russ Tedrake < notifications@github.com> wrote:

(to state the obvious, I missed the end of the if in my code below)

On Jul 20, 2015, at 5:14 PM, aespielberg < notifications@github.com

wrote:

Thanks! I will check this out tonight.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake < notifications@github.com

wrote:

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

body.(fn{i}) =
double(subs(body.param_bindings.(fn{i}),poly,pval));

to

body.(fn{i}) =
subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) =
double(body.fn{i});

(caveat — i didn’t test, but suspect it will work).

again, there are a number of places to change this. please send a pull request if it works!

  • Russ

On Jul 19, 2015, at 4:21 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I have a RigidBodyManipulator - simply, it is an Acrobot with parameters for the two link lengths (I can post it if it helps).

I am trying to do some stuff similar to what's happening in ParameterEstimation(). In particular, I want to set the parameters of my rigid body to be msspoly (or TrigPoly), and from that, calculate either the plant's dynamics using the dynamics() function. Thus, I try:

params_ = p.getParamFrame.getPoly

[ p1 ] [ p2 ]

p = p.setParams(params) Error using msspoly/double (line 21) Cannot cast non-constant msspoly to double.

Error in RigidBody/updateParams (line 319) body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model =

applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p);

Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193>.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123041661

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123043167

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124252178

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124255492

.

— Reply to this email directly or view it on GitHub.

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124784295 .

aespielberg commented 9 years ago

Hi all (for anyone I haven't scared off and is reading this),

I am almost done converting all the needed code for computing dynamics to msspoly. I am not sure yet if I am screwing anything else up by doing this (like, non-symbolic computations), and I'm pretty sure I've made some of the code less efficient. I also haven't solved the compilation problem yet (although I suspect it's because some bodies in the robot are all double and some are mixed), and mex compilation is disabled. I have two questions:

  1. Is there a canonical way to take matrix inverses with msspoly's?
  2. If and when I get this working, is there a way I can submit sort of an experimental pull request where the servers can then run tests and it can be checked how badly I may have messed up existing code?

-Andy S.

On Sat, Jul 25, 2015 at 12:41 PM, Andrew Spielberg aespielberg@gmail.com wrote:

Looking into it now, this seems potentially bad - msspoly cannot be used with matlab's kronecker product because kron uses reshape with a 4 element array, and spotless is only happy with a 2 element array. I can (and probably will) code up my own version of the kronecker product, but it seems like there has to be a cleaner solution.

-Andy S.

On Fri, Jul 24, 2015 at 11:25 PM, Andrew Spielberg aespielberg@gmail.com wrote:

I think adding ret(16, :) = 1; at the end of dHomogTrans fixes that problem, but I get a new error. Unfortunately I'm out of time for this evening and this problem looks a bit deeper than the others, so I have to leave it here for now. Maybe there are cleaner changes that can be made than all these things I've been doing? Sorry again for so many posts.

Error using msspoly/reshape (line 4)
Single argument must be 1-by-2.

Error in kron (line 35)
   B = reshape(B,[mb 1 nb 1]);

Error in matGradMultMat (line 22)
  B_diag = kron(eye(q),B);

Error in RigidBodyManipulator/doKinematics>computeTransformGradients
(line 346)
  ret{i} = matGradMultMat(...

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273)
    kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
  kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
        [H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 11:14 PM, Andrew Spielberg <aespielberg@gmail.com

wrote:

(Trying to figure out if replacing the numel with: length(T{i}(:)), nq) will work, but dHomogTrans I now realize is returning a 15 x 1 vector when computed with a 4x4 msspoly as opposed to a 4 x 4 double, and I'm not sure why.)

-Andy S.

On Fri, Jul 24, 2015 at 11:00 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Sorry for so many comments in a row: right now I'm just making my "best possible fixes" even if they're not the right ones just to see what else breaks down the pipeline or if I can get a bandaged up version of this working.

On line 340 of doKinematics, I replaced:

T_body_to_parent = T{body.parent} \ T{i};

with:

T_body_to_parent = T{body.parent}^-1 * T{i};

Unfortunately now at line 345, I get:

Error using msspoly/subsasgn (line 80)
Subscripted assignment dimension mismatch.

Error in RigidBodyManipulator/doKinematics>computeTransformGradients
(line 345)
  dT_body_to_parentdq(:, body.position_num) = dT_body_to_parentdqi;

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273)
    kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
  kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
        [H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

The RHS is 2x1 while the LHS is 16 x 1. I believe this is because as a double, numel returns 16, but on msspoly, it returns 1 (not sure why). I can write a hack to fix that though, so I'll try to do that for now. I am worried about how deep this rabbit hole goes, though.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 10:45 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

One other problem: For now I decided to just comment out the offending mex line (dunno if that messes anything up but I decided to do that just so I could test out more things). However, when I then try to query the dynamics, I've run into this problem:

[xdot, dxdot] = p.dynamics(0, [pi; pi; 0.1; 0.1], [0.4]) Undefined operator '\' for input arguments of type 'msspoly'.

Error in RigidBodyManipulator/doKinematics>computeTransformGradients (line 340) T_body_to_parent = T{body.parent} \ T{i};

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273) kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S, kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81) kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25) kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56) [H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

msspoly can't use \, which is worrisome. Is there a canonical way to solve linear systems with msspoly's?

-Andy S.

On Fri, Jul 24, 2015 at 10:22 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

I think I'm getting closer - follow-up: what happens if your plant is composed of several bodies, and some of them have doubles for, say, Xtree, and some of them have msspolys? It seems like that might be something that would break it, if it only looks at one of the bodies to determine what to do? Or is there a nested compilation procedure that handles each body individually?

Aside: I've been walking through the entire compilation procedure - one thing I"m not sure about is, what does constructParamFrame do? It returns a paramFrame with length1 and length2 but since I'm passing it new parameters which are msspoly [p1; p2], shouldn't it be p1 and p2?

-Andy S.

On Fri, Jul 24, 2015 at 9:32 PM, Russ Tedrake < notifications@github.com> wrote:

That should be fine. We normally check the type of x (not the params) to decide whether to route to mex. many methods have an option use_mex that you could set to false if your x is a double. If you stay in matlab, then you should get msspoly's back.

On Jul 24, 2015, at 8:27 PM, aespielberg notifications@github.com wrote:

Quick question - what is the expected behavior if part of the system is msspoly, and part of it is double? For example, if mass, laddedmass, and T_body_to_joint are double and the rest of the system is msspoly?

-Andy S.

On Fri, Jul 24, 2015 at 12:21 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Thanks for the info. So if I'm understanding correctly, mex files should never be getting called, so something is messing up before then.

I will look into where the bug is for this tonight. Hopefully I can get everything fixed up by tomorrow.

-Andy S.

On Thu, Jul 23, 2015 at 6:29 PM, Russ Tedrake < notifications@github.com> wrote:

Short answer: ideally c++ files should have no impact on your for now (although your use case is potentially in danger if we make the cleanup that removes the code duplication between matlab and c++). if your t,x,or u are msspoly’s, then it calls the matlab version of kinematics/dynamics. if they are doubles, then it calls the c++ implementation.

Every system has to implement setParameters() in order to do what you do. The RigidBodyManipulator implements it by updating the parameters in the associated RigidBody class. Based on our previous thread, I think people have not tried setting the parameters themselves to be msspolys, but that absolutely should work.

On Jul 23, 2015, at 6:10 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I am hoping to spend my weekend getting analytical, parameterized gradients of my RigidBody. I'd like to help fixing the functionality of RigidBody as much as I can here, but I'm not sure where to start. Would someone be able to provide me a high-level overview of how msspoly's are handled in the non-RigidBody case, and how the RigidBody case is different? Is it that the standard Manipulator is abstract and therefore parameters are just overriding user-defined variables, and the user is then responsible for how to use them, and that the rigidbody tries to do this with links and joints, so it intends to implement as much of this functionality automatically? What role do the C++ files have in all of this implementation?

-Andy S.

On Mon, Jul 20, 2015 at 10:08 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

This didn't work. Here are the changes I made:

line 319, the changes you said. line 35 of rigidbodyelement I made:

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) =
double(body.fn{i}); end

and line 73 of RigidBodyFrame I made:

obj.T = subs(obj.param_binding_T, poly, pval);
if (deg(obj.T) == 0) obj.T = double(obj.T); end

Then, I had an error which complained that isnan is not a member of msspoly in valuecheck.m at line 23. Thus, I inserted:

val = double(val);

on line 22.

Now, I am stuck with the following error:

p.setParams(param_frame.getPoly)
Error using constructModelmex
mxGetPr can only be called on arguments which correspond to
Matlab
doubles

Error in RigidBodyManipulator/createMexPointer (line 2076)
obj.mex_model_ptr = constructModelmex(obj);

Error in RigidBodyManipulator/compile (line 807)
model = createMexPointer(model);

Error in RigidBodyManipulator/setParams (line 1055)
model = compile(model);

Assuming everything I did prior is kosher, I worry this means that something in the C++ mex files are not playing well with parameters. Any advice on how to resolve this?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:19 PM, Russ Tedrake < notifications@github.com> wrote:

(to state the obvious, I missed the end of the if in my code below)

On Jul 20, 2015, at 5:14 PM, aespielberg < notifications@github.com

wrote:

Thanks! I will check this out tonight.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake < notifications@github.com

wrote:

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

body.(fn{i}) =
double(subs(body.param_bindings.(fn{i}),poly,pval));

to

body.(fn{i}) =
subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) =
double(body.fn{i});

(caveat — i didn’t test, but suspect it will work).

again, there are a number of places to change this. please send a pull request if it works!

  • Russ

On Jul 19, 2015, at 4:21 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I have a RigidBodyManipulator - simply, it is an Acrobot with parameters for the two link lengths (I can post it if it helps).

I am trying to do some stuff similar to what's happening in ParameterEstimation(). In particular, I want to set the parameters of my rigid body to be msspoly (or TrigPoly), and from that, calculate either the plant's dynamics using the dynamics() function. Thus, I try:

params_ = p.getParamFrame.getPoly

[ p1 ] [ p2 ]

p = p.setParams(params) Error using msspoly/double (line 21) Cannot cast non-constant msspoly to double.

Error in RigidBody/updateParams (line 319) body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model =

applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p);

Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193>.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123041661

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123043167

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124252178

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124255492

.

— Reply to this email directly or view it on GitHub.

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124784295 .

RussTedrake commented 9 years ago
  1. Is there a canonical way to take matrix inverses with msspoly’s?

no. the result would not be polynomial.

  1. If and when I get this working, is there a way I can submit sort of an experimental pull request where the servers can then run tests and it can be checked how badly I may have messed up existing code?

yes. submitting the pull request on github will do exactly that.

-Andy S.

On Sat, Jul 25, 2015 at 12:41 PM, Andrew Spielberg aespielberg@gmail.com wrote:

Looking into it now, this seems potentially bad - msspoly cannot be used with matlab's kronecker product because kron uses reshape with a 4 element array, and spotless is only happy with a 2 element array. I can (and probably will) code up my own version of the kronecker product, but it seems like there has to be a cleaner solution.

-Andy S.

On Fri, Jul 24, 2015 at 11:25 PM, Andrew Spielberg aespielberg@gmail.com wrote:

I think adding ret(16, :) = 1; at the end of dHomogTrans fixes that problem, but I get a new error. Unfortunately I'm out of time for this evening and this problem looks a bit deeper than the others, so I have to leave it here for now. Maybe there are cleaner changes that can be made than all these things I've been doing? Sorry again for so many posts.

Error using msspoly/reshape (line 4)
Single argument must be 1-by-2.

Error in kron (line 35)
B = reshape(B,[mb 1 nb 1]);

Error in matGradMultMat (line 22)
B_diag = kron(eye(q),B);

Error in RigidBodyManipulator/doKinematics>computeTransformGradients
(line 346)
ret{i} = matGradMultMat(...

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273)
kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
[H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 11:14 PM, Andrew Spielberg <aespielberg@gmail.com

wrote:

(Trying to figure out if replacing the numel with: length(T{i}(:)), nq) will work, but dHomogTrans I now realize is returning a 15 x 1 vector when computed with a 4x4 msspoly as opposed to a 4 x 4 double, and I'm not sure why.)

-Andy S.

On Fri, Jul 24, 2015 at 11:00 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Sorry for so many comments in a row: right now I'm just making my "best possible fixes" even if they're not the right ones just to see what else breaks down the pipeline or if I can get a bandaged up version of this working.

On line 340 of doKinematics, I replaced:

T_body_to_parent = T{body.parent} \ T{i};

with:

T_body_to_parent = T{body.parent}^-1 * T{i};

Unfortunately now at line 345, I get:

Error using msspoly/subsasgn (line 80)
Subscripted assignment dimension mismatch.

Error in RigidBodyManipulator/doKinematics>computeTransformGradients
(line 345)
dT_body_to_parentdq(:, body.position_num) = dT_body_to_parentdqi;

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273)
kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
[H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

The RHS is 2x1 while the LHS is 16 x 1. I believe this is because as a double, numel returns 16, but on msspoly, it returns 1 (not sure why). I can write a hack to fix that though, so I'll try to do that for now. I am worried about how deep this rabbit hole goes, though.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 10:45 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

One other problem: For now I decided to just comment out the offending mex line (dunno if that messes anything up but I decided to do that just so I could test out more things). However, when I then try to query the dynamics, I've run into this problem:

[xdot, dxdot] = p.dynamics(0, [pi; pi; 0.1; 0.1], [0.4]) Undefined operator '\' for input arguments of type 'msspoly'.

Error in RigidBodyManipulator/doKinematics>computeTransformGradients (line 340) T_body_to_parent = T{body.parent} \ T{i};

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273) kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S, kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81) kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25) kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56) [H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

msspoly can't use \, which is worrisome. Is there a canonical way to solve linear systems with msspoly's?

-Andy S.

On Fri, Jul 24, 2015 at 10:22 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

I think I'm getting closer - follow-up: what happens if your plant is composed of several bodies, and some of them have doubles for, say, Xtree, and some of them have msspolys? It seems like that might be something that would break it, if it only looks at one of the bodies to determine what to do? Or is there a nested compilation procedure that handles each body individually?

Aside: I've been walking through the entire compilation procedure - one thing I"m not sure about is, what does constructParamFrame do? It returns a paramFrame with length1 and length2 but since I'm passing it new parameters which are msspoly [p1; p2], shouldn't it be p1 and p2?

-Andy S.

On Fri, Jul 24, 2015 at 9:32 PM, Russ Tedrake < notifications@github.com> wrote:

That should be fine. We normally check the type of x (not the params) to decide whether to route to mex. many methods have an option use_mex that you could set to false if your x is a double. If you stay in matlab, then you should get msspoly's back.

On Jul 24, 2015, at 8:27 PM, aespielberg notifications@github.com wrote:

Quick question - what is the expected behavior if part of the system is msspoly, and part of it is double? For example, if mass, laddedmass, and T_body_to_joint are double and the rest of the system is msspoly?

-Andy S.

On Fri, Jul 24, 2015 at 12:21 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Thanks for the info. So if I'm understanding correctly, mex files should never be getting called, so something is messing up before then.

I will look into where the bug is for this tonight. Hopefully I can get everything fixed up by tomorrow.

-Andy S.

On Thu, Jul 23, 2015 at 6:29 PM, Russ Tedrake < notifications@github.com> wrote:

Short answer: ideally c++ files should have no impact on your for now (although your use case is potentially in danger if we make the cleanup that removes the code duplication between matlab and c++). if your t,x,or u are msspoly’s, then it calls the matlab version of kinematics/dynamics. if they are doubles, then it calls the c++ implementation.

Every system has to implement setParameters() in order to do what you do. The RigidBodyManipulator implements it by updating the parameters in the associated RigidBody class. Based on our previous thread, I think people have not tried setting the parameters themselves to be msspolys, but that absolutely should work.

On Jul 23, 2015, at 6:10 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I am hoping to spend my weekend getting analytical, parameterized gradients of my RigidBody. I'd like to help fixing the functionality of RigidBody as much as I can here, but I'm not sure where to start. Would someone be able to provide me a high-level overview of how msspoly's are handled in the non-RigidBody case, and how the RigidBody case is different? Is it that the standard Manipulator is abstract and therefore parameters are just overriding user-defined variables, and the user is then responsible for how to use them, and that the rigidbody tries to do this with links and joints, so it intends to implement as much of this functionality automatically? What role do the C++ files have in all of this implementation?

-Andy S.

On Mon, Jul 20, 2015 at 10:08 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

This didn't work. Here are the changes I made:

line 319, the changes you said. line 35 of rigidbodyelement I made:

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) =
double(body.fn{i}); end

and line 73 of RigidBodyFrame I made:

obj.T = subs(obj.param_binding_T, poly, pval);
if (deg(obj.T) == 0) obj.T = double(obj.T); end

Then, I had an error which complained that isnan is not a member of msspoly in valuecheck.m at line 23. Thus, I inserted:

val = double(val);

on line 22.

Now, I am stuck with the following error:

p.setParams(param_frame.getPoly)
Error using constructModelmex
mxGetPr can only be called on arguments which correspond to
Matlab
doubles

Error in RigidBodyManipulator/createMexPointer (line 2076)
obj.mex_model_ptr = constructModelmex(obj);

Error in RigidBodyManipulator/compile (line 807)
model = createMexPointer(model);

Error in RigidBodyManipulator/setParams (line 1055)
model = compile(model);

Assuming everything I did prior is kosher, I worry this means that something in the C++ mex files are not playing well with parameters. Any advice on how to resolve this?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:19 PM, Russ Tedrake < notifications@github.com> wrote:

(to state the obvious, I missed the end of the if in my code below)

On Jul 20, 2015, at 5:14 PM, aespielberg < notifications@github.com

wrote:

Thanks! I will check this out tonight.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake < notifications@github.com

wrote:

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

body.(fn{i}) =
double(subs(body.param_bindings.(fn{i}),poly,pval));

to

body.(fn{i}) =
subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) =
double(body.fn{i});

(caveat — i didn’t test, but suspect it will work).

again, there are a number of places to change this. please send a pull request if it works!

  • Russ

On Jul 19, 2015, at 4:21 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I have a RigidBodyManipulator - simply, it is an Acrobot with parameters for the two link lengths (I can post it if it helps).

I am trying to do some stuff similar to what's happening in ParameterEstimation(). In particular, I want to set the parameters of my rigid body to be msspoly (or TrigPoly), and from that, calculate either the plant's dynamics using the dynamics() function. Thus, I try:

params_ = p.getParamFrame.getPoly

[ p1 ] [ p2 ]

p = p.setParams(params) Error using msspoly/double (line 21) Cannot cast non-constant msspoly to double.

Error in RigidBody/updateParams (line 319) body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model =

applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p);

Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193>.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123041661

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123043167

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124252178

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124255492

.

— Reply to this email directly or view it on GitHub.

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124784295 .

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124872622.

aespielberg commented 9 years ago

Ah, yes, that makes sense...for some reason I was hoping msspoly could maybe handle fractional polynomials, but I guess not. I guess I may need to look into how this can be done without inverses

Okay, I will look into how I set up a pull request then. Never done it before, so I'll read up and hopefully submit in a couple of hours.

-Andy S.

On Sat, Jul 25, 2015 at 3:15 PM, Russ Tedrake notifications@github.com wrote:

  1. Is there a canonical way to take matrix inverses with msspoly’s?

no. the result would not be polynomial.

  1. If and when I get this working, is there a way I can submit sort of an experimental pull request where the servers can then run tests and it can be checked how badly I may have messed up existing code?

yes. submitting the pull request on github will do exactly that.

-Andy S.

On Sat, Jul 25, 2015 at 12:41 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Looking into it now, this seems potentially bad - msspoly cannot be used with matlab's kronecker product because kron uses reshape with a 4 element array, and spotless is only happy with a 2 element array. I can (and probably will) code up my own version of the kronecker product, but it seems like there has to be a cleaner solution.

-Andy S.

On Fri, Jul 24, 2015 at 11:25 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

I think adding ret(16, :) = 1; at the end of dHomogTrans fixes that problem, but I get a new error. Unfortunately I'm out of time for this evening and this problem looks a bit deeper than the others, so I have to leave it here for now. Maybe there are cleaner changes that can be made than all these things I've been doing? Sorry again for so many posts.

Error using msspoly/reshape (line 4)
Single argument must be 1-by-2.

Error in kron (line 35)
B = reshape(B,[mb 1 nb 1]);

Error in matGradMultMat (line 22)
B_diag = kron(eye(q),B);

Error in RigidBodyManipulator/doKinematics>computeTransformGradients
(line 346)
ret{i} = matGradMultMat(...

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273)
kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
[H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 11:14 PM, Andrew Spielberg < aespielberg@gmail.com

wrote:

(Trying to figure out if replacing the numel with: length(T{i}(:)), nq) will work, but dHomogTrans I now realize is returning a 15 x 1 vector when computed with a 4x4 msspoly as opposed to a 4 x 4 double, and I'm not sure why.)

-Andy S.

On Fri, Jul 24, 2015 at 11:00 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Sorry for so many comments in a row: right now I'm just making my "best possible fixes" even if they're not the right ones just to see what else breaks down the pipeline or if I can get a bandaged up version of this working.

On line 340 of doKinematics, I replaced:

T_body_to_parent = T{body.parent} \ T{i};

with:

T_body_to_parent = T{body.parent}^-1 * T{i};

Unfortunately now at line 345, I get:

Error using msspoly/subsasgn (line 80)
Subscripted assignment dimension mismatch.

Error in RigidBodyManipulator/doKinematics>computeTransformGradients
(line 345)
dT_body_to_parentdq(:, body.position_num) = dT_body_to_parentdqi;

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line
273)
kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
[H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

The RHS is 2x1 while the LHS is 16 x 1. I believe this is because as a double, numel returns 16, but on msspoly, it returns 1 (not sure why). I can write a hack to fix that though, so I'll try to do that for now. I am worried about how deep this rabbit hole goes, though.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 10:45 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

One other problem: For now I decided to just comment out the offending mex line (dunno if that messes anything up but I decided to do that just so I could test out more things). However, when I then try to query the dynamics, I've run into this problem:

[xdot, dxdot] = p.dynamics(0, [pi; pi; 0.1; 0.1], [0.4]) Undefined operator '\' for input arguments of type 'msspoly'.

Error in RigidBodyManipulator/doKinematics>computeTransformGradients (line 340) T_body_to_parent = T{body.parent} \ T{i};

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273) kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S, kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81) kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25) kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56) [H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

msspoly can't use \, which is worrisome. Is there a canonical way to solve linear systems with msspoly's?

-Andy S.

On Fri, Jul 24, 2015 at 10:22 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

I think I'm getting closer - follow-up: what happens if your plant is composed of several bodies, and some of them have doubles for, say, Xtree, and some of them have msspolys? It seems like that might be something that would break it, if it only looks at one of the bodies to determine what to do? Or is there a nested compilation procedure that handles each body individually?

Aside: I've been walking through the entire compilation procedure

one thing I"m not sure about is, what does constructParamFrame do? It returns a paramFrame with length1 and length2 but since I'm passing it new parameters which are msspoly [p1; p2], shouldn't it be p1 and p2?

-Andy S.

On Fri, Jul 24, 2015 at 9:32 PM, Russ Tedrake < notifications@github.com> wrote:

That should be fine. We normally check the type of x (not the params) to decide whether to route to mex. many methods have an option use_mex that you could set to false if your x is a double. If you stay in matlab, then you should get msspoly's back.

On Jul 24, 2015, at 8:27 PM, aespielberg < notifications@github.com> wrote:

Quick question - what is the expected behavior if part of the system is msspoly, and part of it is double? For example, if mass, laddedmass, and T_body_to_joint are double and the rest of the system is msspoly?

-Andy S.

On Fri, Jul 24, 2015 at 12:21 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Thanks for the info. So if I'm understanding correctly, mex files should never be getting called, so something is messing up before then.

I will look into where the bug is for this tonight. Hopefully I can get everything fixed up by tomorrow.

-Andy S.

On Thu, Jul 23, 2015 at 6:29 PM, Russ Tedrake < notifications@github.com> wrote:

Short answer: ideally c++ files should have no impact on your for now (although your use case is potentially in danger if we make the cleanup that removes the code duplication between matlab and c++). if your t,x,or u are msspoly’s, then it calls the matlab version of kinematics/dynamics. if they are doubles, then it calls the c++ implementation.

Every system has to implement setParameters() in order to do what you do. The RigidBodyManipulator implements it by updating the parameters in the associated RigidBody class. Based on our previous thread, I think people have not tried setting the parameters themselves to be msspolys, but that absolutely should work.

On Jul 23, 2015, at 6:10 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I am hoping to spend my weekend getting analytical, parameterized gradients of my RigidBody. I'd like to help fixing the functionality of RigidBody as much as I can here, but I'm not sure where to start. Would someone be able to provide me a high-level overview of how msspoly's are handled in the non-RigidBody case, and how the RigidBody case is different? Is it that the standard Manipulator is abstract and therefore parameters are just overriding user-defined variables, and the user is then responsible for how to use them, and that the rigidbody tries to do this with links and joints, so it intends to implement as much of this functionality automatically? What role do the C++ files have in all of this implementation?

-Andy S.

On Mon, Jul 20, 2015 at 10:08 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

This didn't work. Here are the changes I made:

line 319, the changes you said. line 35 of rigidbodyelement I made:

body.(fn{i}) =
subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) =
double(body.fn{i}); end

and line 73 of RigidBodyFrame I made:

obj.T = subs(obj.param_binding_T, poly, pval);
if (deg(obj.T) == 0) obj.T = double(obj.T); end

Then, I had an error which complained that isnan is not a member of msspoly in valuecheck.m at line 23. Thus, I inserted:

val = double(val);

on line 22.

Now, I am stuck with the following error:

p.setParams(param_frame.getPoly)
Error using constructModelmex
mxGetPr can only be called on arguments which
correspond to
Matlab
doubles

Error in RigidBodyManipulator/createMexPointer (line
2076)
obj.mex_model_ptr = constructModelmex(obj);

Error in RigidBodyManipulator/compile (line 807)
model = createMexPointer(model);

Error in RigidBodyManipulator/setParams (line 1055)
model = compile(model);

Assuming everything I did prior is kosher, I worry this means that something in the C++ mex files are not playing well with parameters. Any advice on how to resolve this?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:19 PM, Russ Tedrake < notifications@github.com> wrote:

(to state the obvious, I missed the end of the if in my code below)

On Jul 20, 2015, at 5:14 PM, aespielberg < notifications@github.com

wrote:

Thanks! I will check this out tonight.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake < notifications@github.com

wrote:

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

body.(fn{i}) =
double(subs(body.param_bindings.(fn{i}),poly,pval));

to

body.(fn{i}) =
subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) =
double(body.fn{i});

(caveat — i didn’t test, but suspect it will work).

again, there are a number of places to change this. please send a pull request if it works!

  • Russ

On Jul 19, 2015, at 4:21 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I have a RigidBodyManipulator - simply, it is an Acrobot with parameters for the two link lengths (I can post it if it helps).

I am trying to do some stuff similar to what's happening in ParameterEstimation(). In particular, I want to set the parameters of my rigid body to be msspoly (or TrigPoly), and from that, calculate either the plant's dynamics using the dynamics() function. Thus, I try:

params_ = p.getParamFrame.getPoly

[ p1 ] [ p2 ]

p = p.setParams(params) Error using msspoly/double (line 21) Cannot cast non-constant msspoly to double.

Error in RigidBody/updateParams (line 319) body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model =

applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p);

Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193>.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123041661

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123043167

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124252178

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124255492

.

— Reply to this email directly or view it on GitHub.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124784295

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124872622 .

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124874328 .

aespielberg commented 9 years ago

Dumb question - what is the advantage of msspoly over matlab's built in sym functionality? It seems that is more general.

-Andy S.

On Sat, Jul 25, 2015 at 3:18 PM, Andrew Spielberg aespielberg@gmail.com wrote:

Ah, yes, that makes sense...for some reason I was hoping msspoly could maybe handle fractional polynomials, but I guess not. I guess I may need to look into how this can be done without inverses

Okay, I will look into how I set up a pull request then. Never done it before, so I'll read up and hopefully submit in a couple of hours.

-Andy S.

On Sat, Jul 25, 2015 at 3:15 PM, Russ Tedrake notifications@github.com wrote:

  1. Is there a canonical way to take matrix inverses with msspoly’s?

no. the result would not be polynomial.

  1. If and when I get this working, is there a way I can submit sort of an experimental pull request where the servers can then run tests and it can be checked how badly I may have messed up existing code?

yes. submitting the pull request on github will do exactly that.

-Andy S.

On Sat, Jul 25, 2015 at 12:41 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Looking into it now, this seems potentially bad - msspoly cannot be used with matlab's kronecker product because kron uses reshape with a 4 element array, and spotless is only happy with a 2 element array. I can (and probably will) code up my own version of the kronecker product, but it seems like there has to be a cleaner solution.

-Andy S.

On Fri, Jul 24, 2015 at 11:25 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

I think adding ret(16, :) = 1; at the end of dHomogTrans fixes that problem, but I get a new error. Unfortunately I'm out of time for this evening and this problem looks a bit deeper than the others, so I have to leave it here for now. Maybe there are cleaner changes that can be made than all these things I've been doing? Sorry again for so many posts.

Error using msspoly/reshape (line 4)
Single argument must be 1-by-2.

Error in kron (line 35)
B = reshape(B,[mb 1 nb 1]);

Error in matGradMultMat (line 22)
B_diag = kron(eye(q),B);

Error in RigidBodyManipulator/doKinematics>computeTransformGradients
(line 346)
ret{i} = matGradMultMat(...

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273)
kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
[H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 11:14 PM, Andrew Spielberg < aespielberg@gmail.com

wrote:

(Trying to figure out if replacing the numel with: length(T{i}(:)), nq) will work, but dHomogTrans I now realize is returning a 15 x 1 vector when computed with a 4x4 msspoly as opposed to a 4 x 4 double, and I'm not sure why.)

-Andy S.

On Fri, Jul 24, 2015 at 11:00 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Sorry for so many comments in a row: right now I'm just making my "best possible fixes" even if they're not the right ones just to see what else breaks down the pipeline or if I can get a bandaged up version of this working.

On line 340 of doKinematics, I replaced:

T_body_to_parent = T{body.parent} \ T{i};

with:

T_body_to_parent = T{body.parent}^-1 * T{i};

Unfortunately now at line 345, I get:

Error using msspoly/subsasgn (line 80)
Subscripted assignment dimension mismatch.

Error in
RigidBodyManipulator/doKinematics>computeTransformGradients
(line 345)
dT_body_to_parentdq(:, body.position_num) = dT_body_to_parentdqi;

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line
273)
kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
[H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

The RHS is 2x1 while the LHS is 16 x 1. I believe this is because as a double, numel returns 16, but on msspoly, it returns 1 (not sure why). I can write a hack to fix that though, so I'll try to do that for now. I am worried about how deep this rabbit hole goes, though.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 10:45 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

One other problem: For now I decided to just comment out the offending mex line (dunno if that messes anything up but I decided to do that just so I could test out more things). However, when I then try to query the dynamics, I've run into this problem:

[xdot, dxdot] = p.dynamics(0, [pi; pi; 0.1; 0.1], [0.4]) Undefined operator '\' for input arguments of type 'msspoly'.

Error in RigidBodyManipulator/doKinematics>computeTransformGradients (line 340) T_body_to_parent = T{body.parent} \ T{i};

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273) kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S, kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81) kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25) kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56) [H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

msspoly can't use \, which is worrisome. Is there a canonical way to solve linear systems with msspoly's?

-Andy S.

On Fri, Jul 24, 2015 at 10:22 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

I think I'm getting closer - follow-up: what happens if your plant is composed of several bodies, and some of them have doubles for, say, Xtree, and some of them have msspolys? It seems like that might be something that would break it, if it only looks at one of the bodies to determine what to do? Or is there a nested compilation procedure that handles each body individually?

Aside: I've been walking through the entire compilation procedure - one thing I"m not sure about is, what does constructParamFrame do? It returns a paramFrame with length1 and length2 but since I'm passing it new parameters which are msspoly [p1; p2], shouldn't it be p1 and p2?

-Andy S.

On Fri, Jul 24, 2015 at 9:32 PM, Russ Tedrake < notifications@github.com> wrote:

That should be fine. We normally check the type of x (not the params) to decide whether to route to mex. many methods have an option use_mex that you could set to false if your x is a double. If you stay in matlab, then you should get msspoly's back.

On Jul 24, 2015, at 8:27 PM, aespielberg < notifications@github.com> wrote:

Quick question - what is the expected behavior if part of the system is msspoly, and part of it is double? For example, if mass, laddedmass, and T_body_to_joint are double and the rest of the system is msspoly?

-Andy S.

On Fri, Jul 24, 2015 at 12:21 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Thanks for the info. So if I'm understanding correctly, mex files should never be getting called, so something is messing up before then.

I will look into where the bug is for this tonight. Hopefully I can get everything fixed up by tomorrow.

-Andy S.

On Thu, Jul 23, 2015 at 6:29 PM, Russ Tedrake < notifications@github.com> wrote:

Short answer: ideally c++ files should have no impact on your for now (although your use case is potentially in danger if we make the cleanup that removes the code duplication between matlab and c++). if your t,x,or u are msspoly’s, then it calls the matlab version of kinematics/dynamics. if they are doubles, then it calls the c++ implementation.

Every system has to implement setParameters() in order to do what you do. The RigidBodyManipulator implements it by updating the parameters in the associated RigidBody class. Based on our previous thread, I think people have not tried setting the parameters themselves to be msspolys, but that absolutely should work.

On Jul 23, 2015, at 6:10 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I am hoping to spend my weekend getting analytical, parameterized gradients of my RigidBody. I'd like to help fixing the functionality of RigidBody as much as I can here, but I'm not sure where to start. Would someone be able to provide me a high-level overview of how msspoly's are handled in the non-RigidBody case, and how the RigidBody case is different? Is it that the standard Manipulator is abstract and therefore parameters are just overriding user-defined variables, and the user is then responsible for how to use them, and that the rigidbody tries to do this with links and joints, so it intends to implement as much of this functionality automatically? What role do the C++ files have in all of this implementation?

-Andy S.

On Mon, Jul 20, 2015 at 10:08 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

This didn't work. Here are the changes I made:

line 319, the changes you said. line 35 of rigidbodyelement I made:

body.(fn{i}) =
subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) =
double(body.fn{i}); end

and line 73 of RigidBodyFrame I made:

obj.T = subs(obj.param_binding_T, poly, pval);
if (deg(obj.T) == 0) obj.T = double(obj.T); end

Then, I had an error which complained that isnan is not a member of msspoly in valuecheck.m at line 23. Thus, I inserted:

val = double(val);

on line 22.

Now, I am stuck with the following error:

p.setParams(param_frame.getPoly)
Error using constructModelmex
mxGetPr can only be called on arguments which
correspond to
Matlab
doubles

Error in RigidBodyManipulator/createMexPointer (line
2076)
obj.mex_model_ptr = constructModelmex(obj);

Error in RigidBodyManipulator/compile (line 807)
model = createMexPointer(model);

Error in RigidBodyManipulator/setParams (line 1055)
model = compile(model);

Assuming everything I did prior is kosher, I worry this means that something in the C++ mex files are not playing well with parameters. Any advice on how to resolve this?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:19 PM, Russ Tedrake < notifications@github.com> wrote:

(to state the obvious, I missed the end of the if in my code below)

On Jul 20, 2015, at 5:14 PM, aespielberg < notifications@github.com

wrote:

Thanks! I will check this out tonight.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake < notifications@github.com

wrote:

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

body.(fn{i}) =
double(subs(body.param_bindings.(fn{i}),poly,pval));

to

body.(fn{i}) =
subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) =
double(body.fn{i});

(caveat — i didn’t test, but suspect it will work).

again, there are a number of places to change this. please send a pull request if it works!

  • Russ

On Jul 19, 2015, at 4:21 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I have a RigidBodyManipulator - simply, it is an Acrobot with parameters for the two link lengths (I can post it if it helps).

I am trying to do some stuff similar to what's happening in ParameterEstimation(). In particular, I want to set the parameters of my rigid body to be msspoly (or TrigPoly), and from that, calculate either the plant's dynamics using the dynamics() function. Thus, I try:

params_ = p.getParamFrame.getPoly

[ p1 ] [ p2 ]

p = p.setParams(params) Error using msspoly/double (line 21) Cannot cast non-constant msspoly to double.

Error in RigidBody/updateParams (line 319) body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model =

applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p);

Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193>.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123041661

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123043167

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124252178

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124255492

.

— Reply to this email directly or view it on GitHub.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124784295

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124872622 .

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124874328 .

RussTedrake commented 9 years ago

The symbolic toolbox doesn't scale to complex systems.

We have a tool in the util folder called "generateGradients" that uses it if you want to try.

On Jul 26, 2015, at 1:02 AM, aespielberg notifications@github.com wrote:

Dumb question - what is the advantage of msspoly over matlab's built in sym functionality? It seems that is more general.

-Andy S.

On Sat, Jul 25, 2015 at 3:18 PM, Andrew Spielberg aespielberg@gmail.com wrote:

Ah, yes, that makes sense...for some reason I was hoping msspoly could maybe handle fractional polynomials, but I guess not. I guess I may need to look into how this can be done without inverses

Okay, I will look into how I set up a pull request then. Never done it before, so I'll read up and hopefully submit in a couple of hours.

-Andy S.

On Sat, Jul 25, 2015 at 3:15 PM, Russ Tedrake notifications@github.com wrote:

  1. Is there a canonical way to take matrix inverses with msspoly’s?

no. the result would not be polynomial.

  1. If and when I get this working, is there a way I can submit sort of an experimental pull request where the servers can then run tests and it can be checked how badly I may have messed up existing code?

yes. submitting the pull request on github will do exactly that.

-Andy S.

On Sat, Jul 25, 2015 at 12:41 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Looking into it now, this seems potentially bad - msspoly cannot be used with matlab's kronecker product because kron uses reshape with a 4 element array, and spotless is only happy with a 2 element array. I can (and probably will) code up my own version of the kronecker product, but it seems like there has to be a cleaner solution.

-Andy S.

On Fri, Jul 24, 2015 at 11:25 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

I think adding ret(16, :) = 1; at the end of dHomogTrans fixes that problem, but I get a new error. Unfortunately I'm out of time for this evening and this problem looks a bit deeper than the others, so I have to leave it here for now. Maybe there are cleaner changes that can be made than all these things I've been doing? Sorry again for so many posts.

Error using msspoly/reshape (line 4)
Single argument must be 1-by-2.

Error in kron (line 35)
B = reshape(B,[mb 1 nb 1]);

Error in matGradMultMat (line 22)
B_diag = kron(eye(q),B);

Error in RigidBodyManipulator/doKinematics>computeTransformGradients
(line 346)
ret{i} = matGradMultMat(...

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273)
kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
[H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 11:14 PM, Andrew Spielberg < aespielberg@gmail.com

wrote:

(Trying to figure out if replacing the numel with: length(T{i}(:)), nq) will work, but dHomogTrans I now realize is returning a 15 x 1 vector when computed with a 4x4 msspoly as opposed to a 4 x 4 double, and I'm not sure why.)

-Andy S.

On Fri, Jul 24, 2015 at 11:00 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Sorry for so many comments in a row: right now I'm just making my "best possible fixes" even if they're not the right ones just to see what else breaks down the pipeline or if I can get a bandaged up version of this working.

On line 340 of doKinematics, I replaced:

T_body_to_parent = T{body.parent} \ T{i};

with:

T_body_to_parent = T{body.parent}^-1 * T{i};

Unfortunately now at line 345, I get:

Error using msspoly/subsasgn (line 80)
Subscripted assignment dimension mismatch.

Error in
RigidBodyManipulator/doKinematics>computeTransformGradients
(line 345)
dT_body_to_parentdq(:, body.position_num) = dT_body_to_parentdqi;

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line
273)
kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
[H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

The RHS is 2x1 while the LHS is 16 x 1. I believe this is because as a double, numel returns 16, but on msspoly, it returns 1 (not sure why). I can write a hack to fix that though, so I'll try to do that for now. I am worried about how deep this rabbit hole goes, though.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 10:45 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

One other problem: For now I decided to just comment out the offending mex line (dunno if that messes anything up but I decided to do that just so I could test out more things). However, when I then try to query the dynamics, I've run into this problem:

[xdot, dxdot] = p.dynamics(0, [pi; pi; 0.1; 0.1], [0.4]) Undefined operator '\' for input arguments of type 'msspoly'.

Error in RigidBodyManipulator/doKinematics>computeTransformGradients (line 340) T_body_to_parent = T{body.parent} \ T{i};

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273) kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S, kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81) kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25) kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56) [H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

msspoly can't use \, which is worrisome. Is there a canonical way to solve linear systems with msspoly's?

-Andy S.

On Fri, Jul 24, 2015 at 10:22 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

I think I'm getting closer - follow-up: what happens if your plant is composed of several bodies, and some of them have doubles for, say, Xtree, and some of them have msspolys? It seems like that might be something that would break it, if it only looks at one of the bodies to determine what to do? Or is there a nested compilation procedure that handles each body individually?

Aside: I've been walking through the entire compilation procedure - one thing I"m not sure about is, what does constructParamFrame do? It returns a paramFrame with length1 and length2 but since I'm passing it new parameters which are msspoly [p1; p2], shouldn't it be p1 and p2?

-Andy S.

On Fri, Jul 24, 2015 at 9:32 PM, Russ Tedrake < notifications@github.com> wrote:

That should be fine. We normally check the type of x (not the params) to decide whether to route to mex. many methods have an option use_mex that you could set to false if your x is a double. If you stay in matlab, then you should get msspoly's back.

On Jul 24, 2015, at 8:27 PM, aespielberg < notifications@github.com> wrote:

Quick question - what is the expected behavior if part of the system is msspoly, and part of it is double? For example, if mass, laddedmass, and T_body_to_joint are double and the rest of the system is msspoly?

-Andy S.

On Fri, Jul 24, 2015 at 12:21 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Thanks for the info. So if I'm understanding correctly, mex files should never be getting called, so something is messing up before then.

I will look into where the bug is for this tonight. Hopefully I can get everything fixed up by tomorrow.

-Andy S.

On Thu, Jul 23, 2015 at 6:29 PM, Russ Tedrake < notifications@github.com> wrote:

Short answer: ideally c++ files should have no impact on your for now (although your use case is potentially in danger if we make the cleanup that removes the code duplication between matlab and c++). if your t,x,or u are msspoly’s, then it calls the matlab version of kinematics/dynamics. if they are doubles, then it calls the c++ implementation.

Every system has to implement setParameters() in order to do what you do. The RigidBodyManipulator implements it by updating the parameters in the associated RigidBody class. Based on our previous thread, I think people have not tried setting the parameters themselves to be msspolys, but that absolutely should work.

On Jul 23, 2015, at 6:10 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I am hoping to spend my weekend getting analytical, parameterized gradients of my RigidBody. I'd like to help fixing the functionality of RigidBody as much as I can here, but I'm not sure where to start. Would someone be able to provide me a high-level overview of how msspoly's are handled in the non-RigidBody case, and how the RigidBody case is different? Is it that the standard Manipulator is abstract and therefore parameters are just overriding user-defined variables, and the user is then responsible for how to use them, and that the rigidbody tries to do this with links and joints, so it intends to implement as much of this functionality automatically? What role do the C++ files have in all of this implementation?

-Andy S.

On Mon, Jul 20, 2015 at 10:08 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

This didn't work. Here are the changes I made:

line 319, the changes you said. line 35 of rigidbodyelement I made:

body.(fn{i}) =
subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) =
double(body.fn{i}); end

and line 73 of RigidBodyFrame I made:

obj.T = subs(obj.param_binding_T, poly, pval);
if (deg(obj.T) == 0) obj.T = double(obj.T); end

Then, I had an error which complained that isnan is not a member of msspoly in valuecheck.m at line 23. Thus, I inserted:

val = double(val);

on line 22.

Now, I am stuck with the following error:

p.setParams(param_frame.getPoly)
Error using constructModelmex
mxGetPr can only be called on arguments which
correspond to
Matlab
doubles

Error in RigidBodyManipulator/createMexPointer (line
2076)
obj.mex_model_ptr = constructModelmex(obj);

Error in RigidBodyManipulator/compile (line 807)
model = createMexPointer(model);

Error in RigidBodyManipulator/setParams (line 1055)
model = compile(model);

Assuming everything I did prior is kosher, I worry this means that something in the C++ mex files are not playing well with parameters. Any advice on how to resolve this?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:19 PM, Russ Tedrake < notifications@github.com> wrote:

(to state the obvious, I missed the end of the if in my code below)

On Jul 20, 2015, at 5:14 PM, aespielberg < notifications@github.com

wrote:

Thanks! I will check this out tonight.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake < notifications@github.com

wrote:

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

body.(fn{i}) =
double(subs(body.param_bindings.(fn{i}),poly,pval));

to

body.(fn{i}) =
subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) =
double(body.fn{i});

(caveat — i didn’t test, but suspect it will work).

again, there are a number of places to change this. please send a pull request if it works!

  • Russ

On Jul 19, 2015, at 4:21 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I have a RigidBodyManipulator - simply, it is an Acrobot with parameters for the two link lengths (I can post it if it helps).

I am trying to do some stuff similar to what's happening in ParameterEstimation(). In particular, I want to set the parameters of my rigid body to be msspoly (or TrigPoly), and from that, calculate either the plant's dynamics using the dynamics() function. Thus, I try:

params_ = p.getParamFrame.getPoly

[ p1 ] [ p2 ]

p = p.setParams(params) Error using msspoly/double (line 21) Cannot cast non-constant msspoly to double.

Error in RigidBody/updateParams (line 319) body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model =

applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p);

Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193>.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123041661

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123043167

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124252178

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124255492

.

— Reply to this email directly or view it on GitHub.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124784295

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124872622 .

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124874328 .

— Reply to this email directly or view it on GitHub.

aespielberg commented 9 years ago

I see. I just tried generateGradients, but it...crashed MATLAB three times in a row, so I'm going to issue a bug report.

But, I don't think that would work in general. If I'm not mistaken, the manipulator equations change when contacts are made, right? So, the dynamics might be fine for the case where I have, say, an Acrobot or Pendulum, but if it's something that requires constant re-evaluation of the generateGradients, that's going to be a problem.

It sounds like msspoly won't work though, since the dynamics are not poly in parameters. There is no way I can use the manipulator equations directly, right? They don't depend on time and I don't think there's anyway to use them to integrate over time without solving for the xdot and dxdot vectors.

My one idea is - I really only need symbolic (rather than poly) computation at the end of the dynamics calculation. Is there any utility for converting msspoly to symbolic? Then I can convert to sym at the end of the computation just to take a few inverses.

Otherwise, do you by any chance have any suggestion of how I can go from here? My whole thing is that I need to calculate \partial{dynamics_i}/ \partial{params_i}. Maybe you know of a cleaner way to do this than the way I'm going?

-Andy S.

RussTedrake commented 9 years ago

H, C, and B are all polynomial in the parameters. That’s how we do the parameter estimation. You just have to avoid taking H inverse.

On Jul 26, 2015, at 11:45 AM, aespielberg notifications@github.com wrote:

I see. I just tried generateGradients, but it...crashed MATLAB three times in a row, so I'm going to issue a bug report.

But, I don't think that would work in general. If I'm not mistaken, the manipulator equations change when contacts are made, right? So, the dynamics might be fine for the case where I have, say, an Acrobot or Pendulum, but if it's something that requires constant re-evaluation of the generateGradients, that's going to be a problem.

It sounds like msspoly won't work though, since the dynamics are not poly in parameters. There is no way I can use the manipulator equations directly, right? They don't depend on time and I don't think there's anyway to use them to integrate over time without solving for the xdot and dxdot vectors.

My one idea is - I really only need symbolic (rather than poly) computation at the end of the dynamics calculation. Is there any utility for converting msspoly to symbolic? Then I can convert to sym at the end of the computation just to take a few inverses.

Otherwise, do you by any chance have any suggestion of how I can go from here? My whole thing is that I need to calculate \partial{dynamics_i}/ \partial{params_i}. Maybe you know of a cleaner way to do this than the way I'm going?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Sun, Jul 26, 2015 at 6:44 AM, Russ Tedrake notifications@github.com wrote:

The symbolic toolbox doesn't scale to complex systems.

We have a tool in the util folder called "generateGradients" that uses it if you want to try.

On Jul 26, 2015, at 1:02 AM, aespielberg notifications@github.com wrote:

Dumb question - what is the advantage of msspoly over matlab's built in sym functionality? It seems that is more general.

-Andy S.

On Sat, Jul 25, 2015 at 3:18 PM, Andrew Spielberg <aespielberg@gmail.com

wrote:

Ah, yes, that makes sense...for some reason I was hoping msspoly could maybe handle fractional polynomials, but I guess not. I guess I may need to look into how this can be done without inverses

Okay, I will look into how I set up a pull request then. Never done it before, so I'll read up and hopefully submit in a couple of hours.

-Andy S.

On Sat, Jul 25, 2015 at 3:15 PM, Russ Tedrake < notifications@github.com> wrote:

  1. Is there a canonical way to take matrix inverses with msspoly’s?

no. the result would not be polynomial.

  1. If and when I get this working, is there a way I can submit sort of an experimental pull request where the servers can then run tests and it can be checked how badly I may have messed up existing code?

yes. submitting the pull request on github will do exactly that.

-Andy S.

On Sat, Jul 25, 2015 at 12:41 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Looking into it now, this seems potentially bad - msspoly cannot be used with matlab's kronecker product because kron uses reshape with a 4 element array, and spotless is only happy with a 2 element array. I can (and probably will) code up my own version of the kronecker product, but it seems like there has to be a cleaner solution.

-Andy S.

On Fri, Jul 24, 2015 at 11:25 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

I think adding ret(16, :) = 1; at the end of dHomogTrans fixes that problem, but I get a new error. Unfortunately I'm out of time for this evening and this problem looks a bit deeper than the others, so I have to leave it here for now. Maybe there are cleaner changes that can be made than all these things I've been doing? Sorry again for so many posts.

Error using msspoly/reshape (line 4)
Single argument must be 1-by-2.

Error in kron (line 35)
B = reshape(B,[mb 1 nb 1]);

Error in matGradMultMat (line 22)
B_diag = kron(eye(q),B);

Error in
RigidBodyManipulator/doKinematics>computeTransformGradients
(line 346)
ret{i} = matGradMultMat(...

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line
273)
kinsol.dTdq = computeTransformGradients(bodies, kinsol.T,
kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
[H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 11:14 PM, Andrew Spielberg < aespielberg@gmail.com

wrote:

(Trying to figure out if replacing the numel with: length(T{i}(:)), nq) will work, but dHomogTrans I now realize is returning a 15 x 1 vector when computed with a 4x4 msspoly as opposed to a 4 x 4 double, and I'm not sure why.)

-Andy S.

On Fri, Jul 24, 2015 at 11:00 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Sorry for so many comments in a row: right now I'm just making my "best possible fixes" even if they're not the right ones just to see what else breaks down the pipeline or if I can get a bandaged up version of this working.

On line 340 of doKinematics, I replaced:

T_body_to_parent = T{body.parent} \ T{i};

with:

T_body_to_parent = T{body.parent}^-1 * T{i};

Unfortunately now at line 345, I get:

Error using msspoly/subsasgn (line 80)
Subscripted assignment dimension mismatch.

Error in
RigidBodyManipulator/doKinematics>computeTransformGradients
(line 345)
dT_body_to_parentdq(:, body.position_num) =
dT_body_to_parentdqi;

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew
(line
273)
kinsol.dTdq = computeTransformGradients(bodies, kinsol.T,
kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
[H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

The RHS is 2x1 while the LHS is 16 x 1. I believe this is because as a double, numel returns 16, but on msspoly, it returns 1 (not sure why). I can write a hack to fix that though, so I'll try to do that for now. I am worried about how deep this rabbit hole goes, though.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 10:45 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

One other problem: For now I decided to just comment out the offending mex line (dunno if that messes anything up but I decided to do that just so I could test out more things). However, when I then try to query the dynamics, I've run into this problem:

[xdot, dxdot] = p.dynamics(0, [pi; pi; 0.1; 0.1], [0.4]) Undefined operator '\' for input arguments of type 'msspoly'.

Error in RigidBodyManipulator/doKinematics>computeTransformGradients (line 340) T_body_to_parent = T{body.parent} \ T{i};

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273) kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S, kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81) kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25) kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56) [H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

msspoly can't use \, which is worrisome. Is there a canonical way to solve linear systems with msspoly's?

-Andy S.

On Fri, Jul 24, 2015 at 10:22 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

I think I'm getting closer - follow-up: what happens if your plant is composed of several bodies, and some of them have doubles for, say, Xtree, and some of them have msspolys? It seems like that might be something that would break it, if it only looks at one of the bodies to determine what to do? Or is there a nested compilation procedure that handles each body individually?

Aside: I've been walking through the entire compilation procedure - one thing I"m not sure about is, what does constructParamFrame do? It returns a paramFrame with length1 and length2 but since I'm passing it new parameters which are msspoly [p1; p2], shouldn't it be p1 and p2?

-Andy S.

On Fri, Jul 24, 2015 at 9:32 PM, Russ Tedrake < notifications@github.com> wrote:

That should be fine. We normally check the type of x (not the params) to decide whether to route to mex. many methods have an option use_mex that you could set to false if your x is a double. If you stay in matlab, then you should get msspoly's back.

On Jul 24, 2015, at 8:27 PM, aespielberg < notifications@github.com> wrote:

Quick question - what is the expected behavior if part of the system is msspoly, and part of it is double? For example, if mass, laddedmass, and T_body_to_joint are double and the rest of the system is msspoly?

-Andy S.

On Fri, Jul 24, 2015 at 12:21 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Thanks for the info. So if I'm understanding correctly, mex files should never be getting called, so something is messing up before then.

I will look into where the bug is for this tonight. Hopefully I can get everything fixed up by tomorrow.

-Andy S.

On Thu, Jul 23, 2015 at 6:29 PM, Russ Tedrake < notifications@github.com> wrote:

Short answer: ideally c++ files should have no impact on your for now (although your use case is potentially in danger if we make the cleanup that removes the code duplication between matlab and c++). if your t,x,or u are msspoly’s, then it calls the matlab version of kinematics/dynamics. if they are doubles, then it calls the c++ implementation.

Every system has to implement setParameters() in order to do what you do. The RigidBodyManipulator implements it by updating the parameters in the associated RigidBody class. Based on our previous thread, I think people have not tried setting the parameters themselves to be msspolys, but that absolutely should work.

On Jul 23, 2015, at 6:10 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I am hoping to spend my weekend getting analytical, parameterized gradients of my RigidBody. I'd like to help fixing the functionality of RigidBody as much as I can here, but I'm not sure where to start. Would someone be able to provide me a high-level overview of how msspoly's are handled in the non-RigidBody case, and how the RigidBody case is different? Is it that the standard Manipulator is abstract and therefore parameters are just overriding user-defined variables, and the user is then responsible for how to use them, and that the rigidbody tries to do this with links and joints, so it intends to implement as much of this functionality automatically? What role do the C++ files have in all of this implementation?

-Andy S.

On Mon, Jul 20, 2015 at 10:08 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

This didn't work. Here are the changes I made:

line 319, the changes you said. line 35 of rigidbodyelement I made:

body.(fn{i}) =
subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) =
double(body.fn{i}); end

and line 73 of RigidBodyFrame I made:

obj.T = subs(obj.param_binding_T, poly, pval);
if (deg(obj.T) == 0) obj.T = double(obj.T); end

Then, I had an error which complained that isnan is not a member of msspoly in valuecheck.m at line 23. Thus, I inserted:

val = double(val);

on line 22.

Now, I am stuck with the following error:

p.setParams(param_frame.getPoly)
Error using constructModelmex
mxGetPr can only be called on arguments which
correspond to
Matlab
doubles

Error in RigidBodyManipulator/createMexPointer
(line
2076)
obj.mex_model_ptr = constructModelmex(obj);

Error in RigidBodyManipulator/compile (line 807)
model = createMexPointer(model);

Error in RigidBodyManipulator/setParams (line 1055)
model = compile(model);

Assuming everything I did prior is kosher, I worry this means that something in the C++ mex files are not playing well with parameters. Any advice on how to resolve this?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:19 PM, Russ Tedrake < notifications@github.com> wrote:

(to state the obvious, I missed the end of the if in my code below)

On Jul 20, 2015, at 5:14 PM, aespielberg < notifications@github.com

wrote:

Thanks! I will check this out tonight.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake < notifications@github.com

wrote:

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

body.(fn{i}) =
double(subs(body.param_bindings.(fn{i}),poly,pval));

to

body.(fn{i}) =
subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) =
double(body.fn{i});

(caveat — i didn’t test, but suspect it will work).

again, there are a number of places to change this. please send a pull request if it works!

  • Russ

On Jul 19, 2015, at 4:21 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I have a RigidBodyManipulator - simply, it is an Acrobot with parameters for the two link lengths (I can post it if it helps).

I am trying to do some stuff similar to what's happening in ParameterEstimation(). In particular, I want to set the parameters of my rigid body to be msspoly (or TrigPoly), and from that, calculate either the plant's dynamics using the dynamics() function. Thus, I try:

params_ = p.getParamFrame.getPoly

[ p1 ] [ p2 ]

p = p.setParams(params) Error using msspoly/double (line 21) Cannot cast non-constant msspoly to double.

Error in RigidBody/updateParams (line 319) body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model =

applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p);

Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193>.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123041661

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123043167

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124252178

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124255492

.

— Reply to this email directly or view it on GitHub.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124784295

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124872622

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124874328

.

— Reply to this email directly or view it on GitHub.

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124968933 .

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-125010022.

aespielberg commented 9 years ago

Yep, I'm aware that that's the big problem, but I'm not sure how I can formulate this constraint without taking the H inverse. I guess one thing might be to formulate the constraint left-multiplied by H, and since H is full rank, this should be an if and only if - that the dynamics are satisfied iff H*(dynamics) is satisfied. Does that sound accurate?

-Andy S.

On Sun, Jul 26, 2015 at 11:47 AM, Russ Tedrake notifications@github.com wrote:

H, C, and B are all polynomial in the parameters. That’s how we do the parameter estimation. You just have to avoid taking H inverse.

On Jul 26, 2015, at 11:45 AM, aespielberg notifications@github.com wrote:

I see. I just tried generateGradients, but it...crashed MATLAB three times in a row, so I'm going to issue a bug report.

But, I don't think that would work in general. If I'm not mistaken, the manipulator equations change when contacts are made, right? So, the dynamics might be fine for the case where I have, say, an Acrobot or Pendulum, but if it's something that requires constant re-evaluation of the generateGradients, that's going to be a problem.

It sounds like msspoly won't work though, since the dynamics are not poly in parameters. There is no way I can use the manipulator equations directly, right? They don't depend on time and I don't think there's anyway to use them to integrate over time without solving for the xdot and dxdot vectors.

My one idea is - I really only need symbolic (rather than poly) computation at the end of the dynamics calculation. Is there any utility for converting msspoly to symbolic? Then I can convert to sym at the end of the computation just to take a few inverses.

Otherwise, do you by any chance have any suggestion of how I can go from here? My whole thing is that I need to calculate \partial{dynamics_i}/ \partial{params_i}. Maybe you know of a cleaner way to do this than the way I'm going?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Sun, Jul 26, 2015 at 6:44 AM, Russ Tedrake notifications@github.com wrote:

The symbolic toolbox doesn't scale to complex systems.

We have a tool in the util folder called "generateGradients" that uses it if you want to try.

On Jul 26, 2015, at 1:02 AM, aespielberg notifications@github.com wrote:

Dumb question - what is the advantage of msspoly over matlab's built in sym functionality? It seems that is more general.

-Andy S.

On Sat, Jul 25, 2015 at 3:18 PM, Andrew Spielberg < aespielberg@gmail.com

wrote:

Ah, yes, that makes sense...for some reason I was hoping msspoly could maybe handle fractional polynomials, but I guess not. I guess I may need to look into how this can be done without inverses

Okay, I will look into how I set up a pull request then. Never done it before, so I'll read up and hopefully submit in a couple of hours.

-Andy S.

On Sat, Jul 25, 2015 at 3:15 PM, Russ Tedrake < notifications@github.com> wrote:

  1. Is there a canonical way to take matrix inverses with msspoly’s?

no. the result would not be polynomial.

  1. If and when I get this working, is there a way I can submit sort of an experimental pull request where the servers can then run tests and it can be checked how badly I may have messed up existing code?

yes. submitting the pull request on github will do exactly that.

-Andy S.

On Sat, Jul 25, 2015 at 12:41 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Looking into it now, this seems potentially bad - msspoly cannot be used with matlab's kronecker product because kron uses reshape with a 4 element array, and spotless is only happy with a 2 element array. I can (and probably will) code up my own version of the kronecker product, but it seems like there has to be a cleaner solution.

-Andy S.

On Fri, Jul 24, 2015 at 11:25 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

I think adding ret(16, :) = 1; at the end of dHomogTrans fixes that problem, but I get a new error. Unfortunately I'm out of time for this evening and this problem looks a bit deeper than the others, so I have to leave it here for now. Maybe there are cleaner changes that can be made than all these things I've been doing? Sorry again for so many posts.

Error using msspoly/reshape (line 4)
Single argument must be 1-by-2.

Error in kron (line 35)
B = reshape(B,[mb 1 nb 1]);

Error in matGradMultMat (line 22)
B_diag = kron(eye(q),B);

Error in
RigidBodyManipulator/doKinematics>computeTransformGradients
(line 346)
ret{i} = matGradMultMat(...

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew
(line
273)
kinsol.dTdq = computeTransformGradients(bodies, kinsol.T,
kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
[H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 11:14 PM, Andrew Spielberg < aespielberg@gmail.com

wrote:

(Trying to figure out if replacing the numel with: length(T{i}(:)), nq) will work, but dHomogTrans I now realize is returning a 15 x 1 vector when computed with a 4x4 msspoly as opposed to a 4 x 4 double, and I'm not sure why.)

-Andy S.

On Fri, Jul 24, 2015 at 11:00 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Sorry for so many comments in a row: right now I'm just making my "best possible fixes" even if they're not the right ones just to see what else breaks down the pipeline or if I can get a bandaged up version of this working.

On line 340 of doKinematics, I replaced:

T_body_to_parent = T{body.parent} \ T{i};

with:

T_body_to_parent = T{body.parent}^-1 * T{i};

Unfortunately now at line 345, I get:

Error using msspoly/subsasgn (line 80)
Subscripted assignment dimension mismatch.

Error in
RigidBodyManipulator/doKinematics>computeTransformGradients
(line 345)
dT_body_to_parentdq(:, body.position_num) =
dT_body_to_parentdqi;

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew
(line
273)
kinsol.dTdq = computeTransformGradients(bodies, kinsol.T,
kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
[H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

The RHS is 2x1 while the LHS is 16 x 1. I believe this is because as a double, numel returns 16, but on msspoly, it returns 1 (not sure why). I can write a hack to fix that though, so I'll try to do that for now. I am worried about how deep this rabbit hole goes, though.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 10:45 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

One other problem: For now I decided to just comment out the offending mex line (dunno if that messes anything up but I decided to do that just so I could test out more things). However, when I then try to query the dynamics, I've run into this problem:

[xdot, dxdot] = p.dynamics(0, [pi; pi; 0.1; 0.1], [0.4]) Undefined operator '\' for input arguments of type 'msspoly'.

Error in RigidBodyManipulator/doKinematics>computeTransformGradients (line 340) T_body_to_parent = T{body.parent} \ T{i};

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273) kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S, kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81) kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25) kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56) [H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

msspoly can't use \, which is worrisome. Is there a canonical way to solve linear systems with msspoly's?

-Andy S.

On Fri, Jul 24, 2015 at 10:22 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

I think I'm getting closer - follow-up: what happens if your plant is composed of several bodies, and some of them have doubles for, say, Xtree, and some of them have msspolys? It seems like that might be something that would break it, if it only looks at one of the bodies to determine what to do? Or is there a nested compilation procedure that handles each body individually?

Aside: I've been walking through the entire compilation procedure - one thing I"m not sure about is, what does constructParamFrame do? It returns a paramFrame with length1 and length2 but since I'm passing it new parameters which are msspoly [p1; p2], shouldn't it be p1 and p2?

-Andy S.

On Fri, Jul 24, 2015 at 9:32 PM, Russ Tedrake < notifications@github.com> wrote:

That should be fine. We normally check the type of x (not the params) to decide whether to route to mex. many methods have an option use_mex that you could set to false if your x is a double. If you stay in matlab, then you should get msspoly's back.

On Jul 24, 2015, at 8:27 PM, aespielberg < notifications@github.com> wrote:

Quick question - what is the expected behavior if part of the system is msspoly, and part of it is double? For example, if mass, laddedmass, and T_body_to_joint are double and the rest of the system is msspoly?

-Andy S.

On Fri, Jul 24, 2015 at 12:21 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Thanks for the info. So if I'm understanding correctly, mex files should never be getting called, so something is messing up before then.

I will look into where the bug is for this tonight. Hopefully I can get everything fixed up by tomorrow.

-Andy S.

On Thu, Jul 23, 2015 at 6:29 PM, Russ Tedrake < notifications@github.com> wrote:

Short answer: ideally c++ files should have no impact on your for now (although your use case is potentially in danger if we make the cleanup that removes the code duplication between matlab and c++). if your t,x,or u are msspoly’s, then it calls the matlab version of kinematics/dynamics. if they are doubles, then it calls the c++ implementation.

Every system has to implement setParameters() in order to do what you do. The RigidBodyManipulator implements it by updating the parameters in the associated RigidBody class. Based on our previous thread, I think people have not tried setting the parameters themselves to be msspolys, but that absolutely should work.

On Jul 23, 2015, at 6:10 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I am hoping to spend my weekend getting analytical, parameterized gradients of my RigidBody. I'd like to help fixing the functionality of RigidBody as much as I can here, but I'm not sure where to start. Would someone be able to provide me a high-level overview of how msspoly's are handled in the non-RigidBody case, and how the RigidBody case is different? Is it that the standard Manipulator is abstract and therefore parameters are just overriding user-defined variables, and the user is then responsible for how to use them, and that the rigidbody tries to do this with links and joints, so it intends to implement as much of this functionality automatically? What role do the C++ files have in all of this implementation?

-Andy S.

On Mon, Jul 20, 2015 at 10:08 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

This didn't work. Here are the changes I made:

line 319, the changes you said. line 35 of rigidbodyelement I made:

body.(fn{i}) =
subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) =
double(body.fn{i}); end

and line 73 of RigidBodyFrame I made:

obj.T = subs(obj.param_binding_T, poly, pval);
if (deg(obj.T) == 0) obj.T = double(obj.T); end

Then, I had an error which complained that isnan is not a member of msspoly in valuecheck.m at line 23. Thus, I inserted:

val = double(val);

on line 22.

Now, I am stuck with the following error:

p.setParams(param_frame.getPoly)
Error using constructModelmex
mxGetPr can only be called on arguments which
correspond to
Matlab
doubles

Error in RigidBodyManipulator/createMexPointer
(line
2076)
obj.mex_model_ptr = constructModelmex(obj);

Error in RigidBodyManipulator/compile (line
807)
model = createMexPointer(model);

Error in RigidBodyManipulator/setParams (line
1055)
model = compile(model);

Assuming everything I did prior is kosher, I worry this means that something in the C++ mex files are not playing well with parameters. Any advice on how to resolve this?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:19 PM, Russ Tedrake < notifications@github.com> wrote:

(to state the obvious, I missed the end of the if in my code below)

On Jul 20, 2015, at 5:14 PM, aespielberg < notifications@github.com

wrote:

Thanks! I will check this out tonight.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake < notifications@github.com

wrote:

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

body.(fn{i}) =

double(subs(body.param_bindings.(fn{i}),poly,pval));

to

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval); if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i});

(caveat — i didn’t test, but suspect it
will
work).

again, there are a number of places to
change
this.
please send
a pull
request if it works!

- Russ

> On Jul 19, 2015, at 4:21 PM,
aespielberg <
notifications@github.com>
wrote:
>
> Hi all,
>
> I have a RigidBodyManipulator - simply,
it
is an
Acrobot with
parameters
for the two link lengths (I can post it
if it
helps).
>
> I am trying to do some stuff similar to
what's
happening in
ParameterEstimation(). In particular, I
want
to
set the
parameters of
my
rigid body to be msspoly (or TrigPoly),
and
from
that,
calculate
either the
plant's dynamics using the dynamics()
function.
Thus,
I try:
>
> params_ = p.getParamFrame.getPoly
>
> [ p1 ]
> [ p2 ]
>
> >> p_ = p.setParams(params_)
> Error using msspoly/double (line 21)
> Cannot cast non-constant msspoly to
double.
>
> Error in RigidBody/updateParams (line
319)
> body.(fn{i}) =

double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model =

applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p);

Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193>.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123041661

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123043167

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124252178

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124255492

.

— Reply to this email directly or view it on GitHub.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124784295

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124872622

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124874328

.

— Reply to this email directly or view it on GitHub.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124968933

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-125010022 .

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-125010187 .

RussTedrake commented 9 years ago

Correct

On Jul 26, 2015, at 12:00 PM, aespielberg notifications@github.com wrote:

Yep, I'm aware that that's the big problem, but I'm not sure how I can formulate this constraint without taking the H inverse. I guess one thing might be to formulate the constraint left-multiplied by H, and since H is full rank, this should be an if and only if - that the dynamics are satisfied iff H*(dynamics) is satisfied. Does that sound accurate?

-Andy S.

On Sun, Jul 26, 2015 at 11:47 AM, Russ Tedrake notifications@github.com wrote:

H, C, and B are all polynomial in the parameters. That’s how we do the parameter estimation. You just have to avoid taking H inverse.

On Jul 26, 2015, at 11:45 AM, aespielberg notifications@github.com wrote:

I see. I just tried generateGradients, but it...crashed MATLAB three times in a row, so I'm going to issue a bug report.

But, I don't think that would work in general. If I'm not mistaken, the manipulator equations change when contacts are made, right? So, the dynamics might be fine for the case where I have, say, an Acrobot or Pendulum, but if it's something that requires constant re-evaluation of the generateGradients, that's going to be a problem.

It sounds like msspoly won't work though, since the dynamics are not poly in parameters. There is no way I can use the manipulator equations directly, right? They don't depend on time and I don't think there's anyway to use them to integrate over time without solving for the xdot and dxdot vectors.

My one idea is - I really only need symbolic (rather than poly) computation at the end of the dynamics calculation. Is there any utility for converting msspoly to symbolic? Then I can convert to sym at the end of the computation just to take a few inverses.

Otherwise, do you by any chance have any suggestion of how I can go from here? My whole thing is that I need to calculate \partial{dynamics_i}/ \partial{params_i}. Maybe you know of a cleaner way to do this than the way I'm going?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Sun, Jul 26, 2015 at 6:44 AM, Russ Tedrake notifications@github.com wrote:

The symbolic toolbox doesn't scale to complex systems.

We have a tool in the util folder called "generateGradients" that uses it if you want to try.

On Jul 26, 2015, at 1:02 AM, aespielberg notifications@github.com wrote:

Dumb question - what is the advantage of msspoly over matlab's built in sym functionality? It seems that is more general.

-Andy S.

On Sat, Jul 25, 2015 at 3:18 PM, Andrew Spielberg < aespielberg@gmail.com

wrote:

Ah, yes, that makes sense...for some reason I was hoping msspoly could maybe handle fractional polynomials, but I guess not. I guess I may need to look into how this can be done without inverses

Okay, I will look into how I set up a pull request then. Never done it before, so I'll read up and hopefully submit in a couple of hours.

-Andy S.

On Sat, Jul 25, 2015 at 3:15 PM, Russ Tedrake < notifications@github.com> wrote:

  1. Is there a canonical way to take matrix inverses with msspoly’s?

no. the result would not be polynomial.

  1. If and when I get this working, is there a way I can submit sort of an experimental pull request where the servers can then run tests and it can be checked how badly I may have messed up existing code?

yes. submitting the pull request on github will do exactly that.

-Andy S.

On Sat, Jul 25, 2015 at 12:41 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Looking into it now, this seems potentially bad - msspoly cannot be used with matlab's kronecker product because kron uses reshape with a 4 element array, and spotless is only happy with a 2 element array. I can (and probably will) code up my own version of the kronecker product, but it seems like there has to be a cleaner solution.

-Andy S.

On Fri, Jul 24, 2015 at 11:25 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

I think adding ret(16, :) = 1; at the end of dHomogTrans fixes that problem, but I get a new error. Unfortunately I'm out of time for this evening and this problem looks a bit deeper than the others, so I have to leave it here for now. Maybe there are cleaner changes that can be made than all these things I've been doing? Sorry again for so many posts.

Error using msspoly/reshape (line 4)
Single argument must be 1-by-2.

Error in kron (line 35)
B = reshape(B,[mb 1 nb 1]);

Error in matGradMultMat (line 22)
B_diag = kron(eye(q),B);

Error in
RigidBodyManipulator/doKinematics>computeTransformGradients
(line 346)
ret{i} = matGradMultMat(...

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew
(line
273)
kinsol.dTdq = computeTransformGradients(bodies, kinsol.T,
kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
[H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 11:14 PM, Andrew Spielberg < aespielberg@gmail.com

wrote:

(Trying to figure out if replacing the numel with: length(T{i}(:)), nq) will work, but dHomogTrans I now realize is returning a 15 x 1 vector when computed with a 4x4 msspoly as opposed to a 4 x 4 double, and I'm not sure why.)

-Andy S.

On Fri, Jul 24, 2015 at 11:00 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Sorry for so many comments in a row: right now I'm just making my "best possible fixes" even if they're not the right ones just to see what else breaks down the pipeline or if I can get a bandaged up version of this working.

On line 340 of doKinematics, I replaced:

T_body_to_parent = T{body.parent} \ T{i};

with:

T_body_to_parent = T{body.parent}^-1 * T{i};

Unfortunately now at line 345, I get:

Error using msspoly/subsasgn (line 80)
Subscripted assignment dimension mismatch.

Error in
RigidBodyManipulator/doKinematics>computeTransformGradients
(line 345)
dT_body_to_parentdq(:, body.position_num) =
dT_body_to_parentdqi;

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew
(line
273)
kinsol.dTdq = computeTransformGradients(bodies, kinsol.T,
kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
[H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

The RHS is 2x1 while the LHS is 16 x 1. I believe this is because as a double, numel returns 16, but on msspoly, it returns 1 (not sure why). I can write a hack to fix that though, so I'll try to do that for now. I am worried about how deep this rabbit hole goes, though.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 10:45 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

One other problem: For now I decided to just comment out the offending mex line (dunno if that messes anything up but I decided to do that just so I could test out more things). However, when I then try to query the dynamics, I've run into this problem:

[xdot, dxdot] = p.dynamics(0, [pi; pi; 0.1; 0.1], [0.4]) Undefined operator '\' for input arguments of type 'msspoly'.

Error in RigidBodyManipulator/doKinematics>computeTransformGradients (line 340) T_body_to_parent = T{body.parent} \ T{i};

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273) kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S, kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81) kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25) kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56) [H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

msspoly can't use \, which is worrisome. Is there a canonical way to solve linear systems with msspoly's?

-Andy S.

On Fri, Jul 24, 2015 at 10:22 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

I think I'm getting closer - follow-up: what happens if your plant is composed of several bodies, and some of them have doubles for, say, Xtree, and some of them have msspolys? It seems like that might be something that would break it, if it only looks at one of the bodies to determine what to do? Or is there a nested compilation procedure that handles each body individually?

Aside: I've been walking through the entire compilation procedure - one thing I"m not sure about is, what does constructParamFrame do? It returns a paramFrame with length1 and length2 but since I'm passing it new parameters which are msspoly [p1; p2], shouldn't it be p1 and p2?

-Andy S.

On Fri, Jul 24, 2015 at 9:32 PM, Russ Tedrake < notifications@github.com> wrote:

That should be fine. We normally check the type of x (not the params) to decide whether to route to mex. many methods have an option use_mex that you could set to false if your x is a double. If you stay in matlab, then you should get msspoly's back.

On Jul 24, 2015, at 8:27 PM, aespielberg < notifications@github.com> wrote:

Quick question - what is the expected behavior if part of the system is msspoly, and part of it is double? For example, if mass, laddedmass, and T_body_to_joint are double and the rest of the system is msspoly?

-Andy S.

On Fri, Jul 24, 2015 at 12:21 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Thanks for the info. So if I'm understanding correctly, mex files should never be getting called, so something is messing up before then.

I will look into where the bug is for this tonight. Hopefully I can get everything fixed up by tomorrow.

-Andy S.

On Thu, Jul 23, 2015 at 6:29 PM, Russ Tedrake < notifications@github.com> wrote:

Short answer: ideally c++ files should have no impact on your for now (although your use case is potentially in danger if we make the cleanup that removes the code duplication between matlab and c++). if your t,x,or u are msspoly’s, then it calls the matlab version of kinematics/dynamics. if they are doubles, then it calls the c++ implementation.

Every system has to implement setParameters() in order to do what you do. The RigidBodyManipulator implements it by updating the parameters in the associated RigidBody class. Based on our previous thread, I think people have not tried setting the parameters themselves to be msspolys, but that absolutely should work.

On Jul 23, 2015, at 6:10 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I am hoping to spend my weekend getting analytical, parameterized gradients of my RigidBody. I'd like to help fixing the functionality of RigidBody as much as I can here, but I'm not sure where to start. Would someone be able to provide me a high-level overview of how msspoly's are handled in the non-RigidBody case, and how the RigidBody case is different? Is it that the standard Manipulator is abstract and therefore parameters are just overriding user-defined variables, and the user is then responsible for how to use them, and that the rigidbody tries to do this with links and joints, so it intends to implement as much of this functionality automatically? What role do the C++ files have in all of this implementation?

-Andy S.

On Mon, Jul 20, 2015 at 10:08 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

This didn't work. Here are the changes I made:

line 319, the changes you said. line 35 of rigidbodyelement I made:

body.(fn{i}) =
subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) =
double(body.fn{i}); end

and line 73 of RigidBodyFrame I made:

obj.T = subs(obj.param_binding_T, poly, pval);
if (deg(obj.T) == 0) obj.T = double(obj.T); end

Then, I had an error which complained that isnan is not a member of msspoly in valuecheck.m at line 23. Thus, I inserted:

val = double(val);

on line 22.

Now, I am stuck with the following error:

p.setParams(param_frame.getPoly)
Error using constructModelmex
mxGetPr can only be called on arguments which
correspond to
Matlab
doubles

Error in RigidBodyManipulator/createMexPointer
(line
2076)
obj.mex_model_ptr = constructModelmex(obj);

Error in RigidBodyManipulator/compile (line
807)
model = createMexPointer(model);

Error in RigidBodyManipulator/setParams (line
1055)
model = compile(model);

Assuming everything I did prior is kosher, I worry this means that something in the C++ mex files are not playing well with parameters. Any advice on how to resolve this?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:19 PM, Russ Tedrake < notifications@github.com> wrote:

(to state the obvious, I missed the end of the if in my code below)

On Jul 20, 2015, at 5:14 PM, aespielberg < notifications@github.com

wrote:

Thanks! I will check this out tonight.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake < notifications@github.com

wrote:

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

body.(fn{i}) =

double(subs(body.param_bindings.(fn{i}),poly,pval));

to

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval); if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i});

(caveat — i didn’t test, but suspect it
will
work).

again, there are a number of places to
change
this.
please send
a pull
request if it works!

- Russ

> On Jul 19, 2015, at 4:21 PM,
aespielberg <
notifications@github.com>
wrote:
>
> Hi all,
>
> I have a RigidBodyManipulator - simply,
it
is an
Acrobot with
parameters
for the two link lengths (I can post it
if it
helps).
>
> I am trying to do some stuff similar to
what's
happening in
ParameterEstimation(). In particular, I
want
to
set the
parameters of
my
rigid body to be msspoly (or TrigPoly),
and
from
that,
calculate
either the
plant's dynamics using the dynamics()
function.
Thus,
I try:
>
> params_ = p.getParamFrame.getPoly
>
> [ p1 ]
> [ p2 ]
>
> >> p_ = p.setParams(params_)
> Error using msspoly/double (line 21)
> Cannot cast non-constant msspoly to
double.
>
> Error in RigidBody/updateParams (line
319)
> body.(fn{i}) =

double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model =

applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p);

Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193>.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123041661

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123043167

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124252178

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124255492

.

— Reply to this email directly or view it on GitHub.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124784295

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124872622

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124874328

.

— Reply to this email directly or view it on GitHub.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124968933

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-125010022 .

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-125010187 .

— Reply to this email directly or view it on GitHub.

aespielberg commented 9 years ago

Thanks. I'm going to submit a new pull request that fixes some bugs I created - basically I became too aggressive converting things to msspoly.

The only problem with going the HDynamics route is that taking the gradient of the HDynamics now requires that the HDynamics be msspoly in q now, because H is a function of q. So my next step will be to make the necessary conversions to allow that.

-Andy S.

On Sun, Jul 26, 2015 at 12:15 PM, Russ Tedrake notifications@github.com wrote:

Correct

On Jul 26, 2015, at 12:00 PM, aespielberg notifications@github.com wrote:

Yep, I'm aware that that's the big problem, but I'm not sure how I can formulate this constraint without taking the H inverse. I guess one thing might be to formulate the constraint left-multiplied by H, and since H is full rank, this should be an if and only if - that the dynamics are satisfied iff H*(dynamics) is satisfied. Does that sound accurate?

-Andy S.

On Sun, Jul 26, 2015 at 11:47 AM, Russ Tedrake <notifications@github.com

wrote:

H, C, and B are all polynomial in the parameters. That’s how we do the parameter estimation. You just have to avoid taking H inverse.

On Jul 26, 2015, at 11:45 AM, aespielberg notifications@github.com wrote:

I see. I just tried generateGradients, but it...crashed MATLAB three times in a row, so I'm going to issue a bug report.

But, I don't think that would work in general. If I'm not mistaken, the manipulator equations change when contacts are made, right? So, the dynamics might be fine for the case where I have, say, an Acrobot or Pendulum, but if it's something that requires constant re-evaluation of the generateGradients, that's going to be a problem.

It sounds like msspoly won't work though, since the dynamics are not poly in parameters. There is no way I can use the manipulator equations directly, right? They don't depend on time and I don't think there's anyway to use them to integrate over time without solving for the xdot and dxdot vectors.

My one idea is - I really only need symbolic (rather than poly) computation at the end of the dynamics calculation. Is there any utility for converting msspoly to symbolic? Then I can convert to sym at the end of the computation just to take a few inverses.

Otherwise, do you by any chance have any suggestion of how I can go from here? My whole thing is that I need to calculate \partial{dynamics_i}/ \partial{params_i}. Maybe you know of a cleaner way to do this than the way I'm going?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Sun, Jul 26, 2015 at 6:44 AM, Russ Tedrake < notifications@github.com> wrote:

The symbolic toolbox doesn't scale to complex systems.

We have a tool in the util folder called "generateGradients" that uses it if you want to try.

On Jul 26, 2015, at 1:02 AM, aespielberg < notifications@github.com> wrote:

Dumb question - what is the advantage of msspoly over matlab's built in sym functionality? It seems that is more general.

-Andy S.

On Sat, Jul 25, 2015 at 3:18 PM, Andrew Spielberg < aespielberg@gmail.com

wrote:

Ah, yes, that makes sense...for some reason I was hoping msspoly could maybe handle fractional polynomials, but I guess not. I guess I may need to look into how this can be done without inverses

Okay, I will look into how I set up a pull request then. Never done it before, so I'll read up and hopefully submit in a couple of hours.

-Andy S.

On Sat, Jul 25, 2015 at 3:15 PM, Russ Tedrake < notifications@github.com> wrote:

  1. Is there a canonical way to take matrix inverses with msspoly’s?

no. the result would not be polynomial.

  1. If and when I get this working, is there a way I can submit sort of an experimental pull request where the servers can then run tests and it can be checked how badly I may have messed up existing code?

yes. submitting the pull request on github will do exactly that.

-Andy S.

On Sat, Jul 25, 2015 at 12:41 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Looking into it now, this seems potentially bad - msspoly cannot be used with matlab's kronecker product because kron uses reshape with a 4 element array, and spotless is only happy with a 2 element array. I can (and probably will) code up my own version of the kronecker product, but it seems like there has to be a cleaner solution.

-Andy S.

On Fri, Jul 24, 2015 at 11:25 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

I think adding ret(16, :) = 1; at the end of dHomogTrans fixes that problem, but I get a new error. Unfortunately I'm out of time for this evening and this problem looks a bit deeper than the others, so I have to leave it here for now. Maybe there are cleaner changes that can be made than all these things I've been doing? Sorry again for so many posts.

Error using msspoly/reshape (line 4)
Single argument must be 1-by-2.

Error in kron (line 35)
B = reshape(B,[mb 1 nb 1]);

Error in matGradMultMat (line 22)
B_diag = kron(eye(q),B);

Error in
RigidBodyManipulator/doKinematics>computeTransformGradients
(line 346)
ret{i} = matGradMultMat(...

Error in
RigidBodyManipulator/doKinematicsNdoKinematicsNew
(line
273)
kinsol.dTdq = computeTransformGradients(bodies, kinsol.T,
kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line
25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
[H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 11:14 PM, Andrew Spielberg < aespielberg@gmail.com

wrote:

(Trying to figure out if replacing the numel with: length(T{i}(:)), nq) will work, but dHomogTrans I now realize is returning a 15 x 1 vector when computed with a 4x4 msspoly as opposed to a 4 x 4 double, and I'm not sure why.)

-Andy S.

On Fri, Jul 24, 2015 at 11:00 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Sorry for so many comments in a row: right now I'm just making my "best possible fixes" even if they're not the right ones just to see what else breaks down the pipeline or if I can get a bandaged up version of this working.

On line 340 of doKinematics, I replaced:

T_body_to_parent = T{body.parent} \ T{i};

with:

T_body_to_parent = T{body.parent}^-1 * T{i};

Unfortunately now at line 345, I get:

Error using msspoly/subsasgn (line 80)
Subscripted assignment dimension mismatch.

Error in
RigidBodyManipulator/doKinematics>computeTransformGradients
(line 345)
dT_body_to_parentdq(:, body.position_num) =
dT_body_to_parentdqi;

Error in
RigidBodyManipulator/doKinematicsNdoKinematicsNew
(line
273)
kinsol.dTdq = computeTransformGradients(bodies,
kinsol.T,
kinsol.S,
kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81)
kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics
(line 25)
kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56)
[H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

The RHS is 2x1 while the LHS is 16 x 1. I believe this is because as a double, numel returns 16, but on msspoly, it returns 1 (not sure why). I can write a hack to fix that though, so I'll try to do that for now. I am worried about how deep this rabbit hole goes, though.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Fri, Jul 24, 2015 at 10:45 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

One other problem: For now I decided to just comment out the offending mex line (dunno if that messes anything up but I decided to do that just so I could test out more things). However, when I then try to query the dynamics, I've run into this problem:

[xdot, dxdot] = p.dynamics(0, [pi; pi; 0.1; 0.1], [0.4]) Undefined operator '\' for input arguments of type 'msspoly'.

Error in RigidBodyManipulator/doKinematics>computeTransformGradients (line 340) T_body_to_parent = T{body.parent} \ T{i};

Error in RigidBodyManipulator/doKinematicsNdoKinematicsNew (line 273) kinsol.dTdq = computeTransformGradients(bodies, kinsol.T, kinsol.S, kinsol.qdotToV, length(q));

Error in RigidBodyManipulator/doKinematics (line 81) kinsol = doKinematicsNew(model, q, v, options);

Error in RigidBodyManipulator/manipulatorDynamics (line 25) kinsol = doKinematics(obj, q, v, options);

Error in Manipulator/dynamics (line 56) [H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,v);

msspoly can't use \, which is worrisome. Is there a canonical way to solve linear systems with msspoly's?

-Andy S.

On Fri, Jul 24, 2015 at 10:22 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

I think I'm getting closer - follow-up: what happens if your plant is composed of several bodies, and some of them have doubles for, say, Xtree, and some of them have msspolys? It seems like that might be something that would break it, if it only looks at one of the bodies to determine what to do? Or is there a nested compilation procedure that handles each body individually?

Aside: I've been walking through the entire compilation procedure - one thing I"m not sure about is, what does constructParamFrame do? It returns a paramFrame with length1 and length2 but since I'm passing it new parameters which are msspoly [p1; p2], shouldn't it be p1 and p2?

-Andy S.

On Fri, Jul 24, 2015 at 9:32 PM, Russ Tedrake < notifications@github.com> wrote:

That should be fine. We normally check the type of x (not the params) to decide whether to route to mex. many methods have an option use_mex that you could set to false if your x is a double. If you stay in matlab, then you should get msspoly's back.

On Jul 24, 2015, at 8:27 PM, aespielberg < notifications@github.com> wrote:

Quick question - what is the expected behavior if part of the system is msspoly, and part of it is double? For example, if mass, laddedmass, and T_body_to_joint are double and the rest of the system is msspoly?

-Andy S.

On Fri, Jul 24, 2015 at 12:21 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

Thanks for the info. So if I'm understanding correctly, mex files should never be getting called, so something is messing up before then.

I will look into where the bug is for this tonight. Hopefully I can get everything fixed up by tomorrow.

-Andy S.

On Thu, Jul 23, 2015 at 6:29 PM, Russ Tedrake < notifications@github.com> wrote:

Short answer: ideally c++ files should have no impact on your for now (although your use case is potentially in danger if we make the cleanup that removes the code duplication between matlab and c++). if your t,x,or u are msspoly’s, then it calls the matlab version of kinematics/dynamics. if they are doubles, then it calls the c++ implementation.

Every system has to implement setParameters() in order to do what you do. The RigidBodyManipulator implements it by updating the parameters in the associated RigidBody class. Based on our previous thread, I think people have not tried setting the parameters themselves to be msspolys, but that absolutely should work.

On Jul 23, 2015, at 6:10 PM, aespielberg < notifications@github.com> wrote:

Hi all,

I am hoping to spend my weekend getting analytical, parameterized gradients of my RigidBody. I'd like to help fixing the functionality of RigidBody as much as I can here, but I'm not sure where to start. Would someone be able to provide me a high-level overview of how msspoly's are handled in the non-RigidBody case, and how the RigidBody case is different? Is it that the standard Manipulator is abstract and therefore parameters are just overriding user-defined variables, and the user is then responsible for how to use them, and that the rigidbody tries to do this with links and joints, so it intends to implement as much of this functionality automatically? What role do the C++ files have in all of this implementation?

-Andy S.

On Mon, Jul 20, 2015 at 10:08 PM, Andrew Spielberg < aespielberg@gmail.com> wrote:

This didn't work. Here are the changes I made:

line 319, the changes you said. line 35 of rigidbodyelement I made:

body.(fn{i}) =
subs(body.param_bindings.(fn{i}),poly,pval);
if (deg(body.(fn{i}))==0) body.(fn{i}) =
double(body.fn{i}); end

and line 73 of RigidBodyFrame I made:

obj.T = subs(obj.param_binding_T, poly,
pval);
if (deg(obj.T) == 0) obj.T =
double(obj.T); end

Then, I had an error which complained that isnan is not a member of msspoly in valuecheck.m at line 23. Thus, I inserted:

val = double(val);

on line 22.

Now, I am stuck with the following error:

p.setParams(param_frame.getPoly)
Error using constructModelmex
mxGetPr can only be called on arguments
which
correspond to
Matlab
doubles

Error in
RigidBodyManipulator/createMexPointer
(line
2076)
obj.mex_model_ptr = constructModelmex(obj);

Error in RigidBodyManipulator/compile (line
807)
model = createMexPointer(model);

Error in RigidBodyManipulator/setParams
(line
1055)
model = compile(model);

Assuming everything I did prior is kosher, I worry this means that something in the C++ mex files are not playing well with parameters. Any advice on how to resolve this?

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:19 PM, Russ Tedrake < notifications@github.com> wrote:

(to state the obvious, I missed the end of the if in my code below)

On Jul 20, 2015, at 5:14 PM, aespielberg < notifications@github.com

wrote:

Thanks! I will check this out tonight.

-Andy S.

Andrew Spielberg PhD Student MIT - Computer Science and Artificial Intelligence Laboratory

On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake < notifications@github.com

wrote:

It’s easy enough to support this case, and we should. It just looks like the cast to double() is happening to quickly in RigidBody/updateParams (and many other updateParams calls in the RigidBodyManipulator classes.

Try changing line 319 from

body.(fn{i}) =

double(subs(body.param_bindings.(fn{i}),poly,pval));

to

body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval); if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i});

(caveat — i didn’t test, but suspect
it
will
work).

again, there are a number of places to
change
this.
please send
a pull
request if it works!

- Russ

> On Jul 19, 2015, at 4:21 PM,
aespielberg <
notifications@github.com>
wrote:
>
> Hi all,
>
> I have a RigidBodyManipulator -
simply,
it
is an
Acrobot with
parameters
for the two link lengths (I can post
it
if it
helps).
>
> I am trying to do some stuff
similar to
what's
happening in
ParameterEstimation(). In particular,
I
want
to
set the
parameters of
my
rigid body to be msspoly (or
TrigPoly),
and
from
that,
calculate
either the
plant's dynamics using the dynamics()
function.
Thus,
I try:
>
> params_ = p.getParamFrame.getPoly
>
> [ p1 ]
> [ p2 ]
>
> >> p_ = p.setParams(params_)
> Error using msspoly/double (line 21)
> Cannot cast non-constant msspoly to
double.
>
> Error in RigidBody/updateParams
(line
319)
> body.(fn{i}) =

double(subs(body.param_bindings.(fn{i}),poly,pval));

Error in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430) model.body(i) = feval(fcn,model.body(i),varargin{:});

Error in RigidBodyManipulator/setParams (line 1053) model =

applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p);

Since this works with AcrobotPlant, and judging by the stacktrace, I'm guessing this means that this is functionality that works for Manipulator but not RigidBodyManipulator. Can anyone recommend a way to get similar functionality with RigidBodyManipulator? My end goal is to get parameterized dynamics (p.dynamics()) in t, x, u, and params.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193>.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123041661

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123043167

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124252178

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124255492

.

— Reply to this email directly or view it on GitHub.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124784295

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124872622

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124874328

.

— Reply to this email directly or view it on GitHub.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124968933

.

— Reply to this email directly or view it on GitHub <

https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-125010022

.

— Reply to this email directly or view it on GitHub < https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-125010187

.

— Reply to this email directly or view it on GitHub.

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-125012722 .

RussTedrake commented 9 years ago

seems like you've made progress on this. will take this off our issue stack. open a new one with any specific questions.