{"action":"created","issue":{"url":"https://api.github.com/repos/RobotLocomotion/drake/issues/1193","labels_url":"https://api.github.com/repos/RobotLocomotion/drake/issues/1193/labels{/name}","comments_url":"https://api.github.com/repos/RobotLocomotion/drake/issues/1193/comments","events_url":"https://api.github.com/repos/RobotLocomotion/drake/issues/1193/events","html_url":"https://github.com/RobotLocomotion/drake/issues/1193","id":95945916,"number":1193,"title":"setParams with RigidBodyManipulator?","user":{"login":"aespielberg","id":3598964,"avatar_url":"https://avatars.githubusercontent.com/u/3598964?v=3","gravatar_id":"","url":"https://api.github.com/users/aespielberg","html_url":"https://github.com/aespielberg","followers_url":"https://api.github.com/users/aespielberg/followers","following_url":"https://api.github.com/users/aespielberg/following{/other_user}","gists_url":"https://api.github.com/users/aespielberg/gists{/gist_id}","starred_url":"https://api.github.com/users/aespielberg/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/aespielberg/subscriptions","organizations_url":"https://api.github.com/users/aespielberg/orgs","repos_url":"https://api.github.com/users/aespielberg/repos","events_url":"https://api.github.com/users/aespielberg/events{/privacy}","received_events_url":"https://api.github.com/users/aespielberg/received_events","type":"User","site_admin":false},"labels":[],"state":"open","locked":false,"assignee":null,"milestone":null,"comments":7,"created_at":"2015-07-19T20:21:21Z","updated_at":"2015-07-24T16:21:21Z","closed_at":null,"body":"Hi all,\r\n\r\nI have a RigidBodyManipulator - simply, it is an Acrobot with parameters for the two link lengths (I can post it if it helps).\r\n\r\nI 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:\r\n\r\n```\r\nparams_ = p.getParamFrame.getPoly\r\n\r\n[   p1  ]\r\n[   p2  ]\r\n\r\n>> p_ = p.setParams(params_)\r\nError using msspoly/double (line 21)\r\nCannot cast non-constant msspoly to double.\r\n\r\nError in RigidBody/updateParams (line 319)\r\n        body.(fn{i}) = double(subs(body.param_bindings.(fn{i}),poly,pval));\r\n\r\nError in RigidBodyManipulator/applyToAllRigidBodyElements (line 2430)\r\n        model.body(i) = feval(fcn,model.body(i),varargin{:});\r\n\r\nError in RigidBodyManipulator/setParams (line 1053)\r\n      model = applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p);\r\n```\r\n\r\nSince 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."},"comment":{"url":"https://api.github.com/repos/RobotLocomotion/drake/issues/comments/124571819","html_url":"https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124571819","issue_url":"https://api.github.com/repos/RobotLocomotion/drake/issues/1193","id":124571819,"user":{"login":"aespielberg","id":3598964,"avatar_url":"https://avatars.githubusercontent.com/u/3598964?v=3","gravatar_id":"","url":"https://api.github.com/users/aespielberg","html_url":"https://github.com/aespielberg","followers_url":"https://api.github.com/users/aespielberg/followers","following_url":"https://api.github.com/users/aespielberg/following{/other_user}","gists_url":"https://api.github.com/users/aespielberg/gists{/gist_id}","starred_url":"https://api.github.com/users/aespielberg/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/aespielberg/subscriptions","organizations_url":"https://api.github.com/users/aespielberg/orgs","repos_url":"https://api.github.com/users/aespielberg/repos","events_url":"https://api.github.com/users/aespielberg/events{/privacy}","received_events_url":"https://api.github.com/users/aespielberg/received_events","type":"User","site_admin":false},"created_at":"2015-07-24T16:21:21Z","updated_at":"2015-07-24T16:21:21Z","body":"Thanks for the info.  So if I'm understanding correctly, mex files should\nnever be getting called, so something is messing up before then.\n\nI will look into where the bug is for this tonight.  Hopefully I can get\neverything fixed up by tomorrow.\n\n-Andy S.\n\nOn Thu, Jul 23, 2015 at 6:29 PM, Russ Tedrake <notifications@github.com>\nwrote:\n\n> Short answer: ideally c++ files should have no impact on your for now\n> (although your use case is potentially in danger if we make the cleanup\n> that removes the code duplication between matlab and c++). if your t,x,or u\n> are msspoly’s, then it calls the matlab version of kinematics/dynamics. if\n> they are doubles, then it calls the c++ implementation.\n>\n> Every system has to implement setParameters() in order to do what you do.\n> The RigidBodyManipulator implements it by updating the parameters in the\n> associated RigidBody class. Based on our previous thread, I think people\n> have not tried setting the parameters themselves to be msspolys, but that\n> absolutely should work.\n>\n>\n>\n> > On Jul 23, 2015, at 6:10 PM, aespielberg <notifications@github.com>\n> wrote:\n> >\n> > Hi all,\n> >\n> > I am hoping to spend my weekend getting analytical, parameterized\n> gradients\n> > of my RigidBody. I'd like to help fixing the functionality of RigidBody\n> as\n> > much as I can here, but I'm not sure where to start. Would someone be\n> able\n> > to provide me a high-level overview of how msspoly's are handled in the\n> > non-RigidBody case, and how the RigidBody case is different? Is it that\n> > the standard Manipulator is abstract and therefore parameters are just\n> > overriding user-defined variables, and the user is then responsible for\n> how\n> > to use them, and that the rigidbody tries to do this with links and\n> joints,\n> > so it intends to implement as much of this functionality automatically?\n> > What role do the C++ files have in all of this implementation?\n> >\n> > -Andy S.\n> >\n> > On Mon, Jul 20, 2015 at 10:08 PM, Andrew Spielberg <\n> aespielberg@gmail.com>\n> > wrote:\n> >\n> > > This didn't work. Here are the changes I made:\n> > >\n> > > line 319, the changes you said.\n> > > line 35 of rigidbodyelement I made:\n> > > ```\n> > > body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);\n> > > if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i}); end\n> > > ```\n> > >\n> > > and line 73 of RigidBodyFrame I made:\n> > >\n> > > ```\n> > > obj.T = subs(obj.param_binding_T, poly, pval);\n> > > if (deg(obj.T) == 0) obj.T = double(obj.T); end\n> > > ```\n> > >\n> > > Then, I had an error which complained that isnan is not a member of\n> > > msspoly in valuecheck.m at line 23. Thus, I inserted:\n> > > ```\n> > > val = double(val);\n> > > ```\n> > > on line 22.\n> > >\n> > > Now, I am stuck with the following error:\n> > >\n> > > ```\n> > > p.setParams(param_frame.getPoly)\n> > > Error using constructModelmex\n> > > mxGetPr can only be called on arguments which correspond to Matlab\n> doubles\n> > >\n> > > Error in RigidBodyManipulator/createMexPointer (line 2076)\n> > > obj.mex_model_ptr = constructModelmex(obj);\n> > >\n> > > Error in RigidBodyManipulator/compile (line 807)\n> > > model = createMexPointer(model);\n> > >\n> > > Error in RigidBodyManipulator/setParams (line 1055)\n> > > model = compile(model);\n> > > ```\n> > >\n> > > Assuming everything I did prior is kosher, I worry this means that\n> > > something in the C++ mex files are not playing well with parameters.\n> Any\n> > > advice on how to resolve this?\n> > >\n> > > -Andy S.\n> > >\n> > > Andrew Spielberg\n> > > PhD Student\n> > > MIT - Computer Science and Artificial Intelligence Laboratory\n> > >\n> > > On Mon, Jul 20, 2015 at 5:19 PM, Russ Tedrake <\n> notifications@github.com>\n> > > wrote:\n> > >\n> > >> (to state the obvious, I missed the end of the if in my code below)\n> > >>\n> > >>\n> > >> > On Jul 20, 2015, at 5:14 PM, aespielberg <notifications@github.com>\n> > >> wrote:\n> > >> >\n> > >> > Thanks! I will check this out tonight.\n> > >> >\n> > >> > -Andy S.\n> > >> >\n> > >> > Andrew Spielberg\n> > >> > PhD Student\n> > >> > MIT - Computer Science and Artificial Intelligence Laboratory\n> > >> >\n> > >> > On Mon, Jul 20, 2015 at 5:10 PM, Russ Tedrake <\n> notifications@github.com\n> > >> >\n> > >> > wrote:\n> > >> >\n> > >> > > It’s easy enough to support this case, and we should. It just\n> looks\n> > >> like\n> > >> > > the cast to double() is happening to quickly in\n> > >> RigidBody/updateParams (and\n> > >> > > many other updateParams calls in the RigidBodyManipulator classes.\n> > >> > >\n> > >> > > Try changing line 319 from\n> > >> > > ```\n> > >> > > body.(fn{i}) =\n> double(subs(body.param_bindings.(fn{i}),poly,pval));\n> > >> > > ```\n> > >> > > to\n> > >> > > ```\n> > >> > > body.(fn{i}) = subs(body.param_bindings.(fn{i}),poly,pval);\n> > >> > > if (deg(body.(fn{i}))==0) body.(fn{i}) = double(body.fn{i});\n> > >> > > ```\n> > >> > > (caveat — i didn’t test, but suspect it will work).\n> > >> > >\n> > >> > > again, there are a number of places to change this. please send a\n> pull\n> > >> > > request if it works!\n> > >> > >\n> > >> > > - Russ\n> > >> > >\n> > >> > >\n> > >> > >\n> > >> > >\n> > >> > >\n> > >> > > > On Jul 19, 2015, at 4:21 PM, aespielberg <\n> notifications@github.com>\n> > >> > > wrote:\n> > >> > > >\n> > >> > > > Hi all,\n> > >> > > >\n> > >> > > > I have a RigidBodyManipulator - simply, it is an Acrobot with\n> > >> parameters\n> > >> > > for the two link lengths (I can post it if it helps).\n> > >> > > >\n> > >> > > > I am trying to do some stuff similar to what's happening in\n> > >> > > ParameterEstimation(). In particular, I want to set the\n> parameters of\n> > >> my\n> > >> > > rigid body to be msspoly (or TrigPoly), and from that, calculate\n> > >> either the\n> > >> > > plant's dynamics using the dynamics() function. Thus, I try:\n> > >> > > >\n> > >> > > > params_ = p.getParamFrame.getPoly\n> > >> > > >\n> > >> > > > [ p1 ]\n> > >> > > > [ p2 ]\n> > >> > > >\n> > >> > > > >> p_ = p.setParams(params_)\n> > >> > > > Error using msspoly/double (line 21)\n> > >> > > > Cannot cast non-constant msspoly to double.\n> > >> > > >\n> > >> > > > Error in RigidBody/updateParams (line 319)\n> > >> > > > body.(fn{i}) =\n> double(subs(body.param_bindings.(fn{i}),poly,pval));\n> > >> > > >\n> > >> > > > Error in RigidBodyManipulator/applyToAllRigidBodyElements (line\n> > >> 2430)\n> > >> > > > model.body(i) = feval(fcn,model.body(i),varargin{:});\n> > >> > > >\n> > >> > > > Error in RigidBodyManipulator/setParams (line 1053)\n> > >> > > > model =\n> > >> applyToAllRigidBodyElements(model,'updateParams',fr.getPoly,p);\n> > >> > > > Since this works with AcrobotPlant, and judging by the\n> stacktrace,\n> > >> I'm\n> > >> > > guessing this means that this is functionality that works for\n> > >> Manipulator\n> > >> > > but not RigidBodyManipulator. Can anyone recommend a way to get\n> > >> similar\n> > >> > > functionality with RigidBodyManipulator? My end goal is to get\n> > >> > > parameterized dynamics (p.dynamics()) in t, x, u, and params.\n> > >> > > >\n> > >> > > > —\n> > >> > > > Reply to this email directly or view it on GitHub <\n> > >> > > https://github.com/RobotLocomotion/drake/issues/1193>.\n> > >> > > >\n> > >> > >\n> > >> > > —\n> > >> > > Reply to this email directly or view it on GitHub\n> > >> > > <\n> > >>\n> https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123040430\n> > >> >\n> > >> > > .\n> > >> > >\n> > >> > —\n> > >> > Reply to this email directly or view it on GitHub <\n> > >>\n> https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123041661\n> > >> >.\n> > >> >\n> > >>\n> > >> —\n> > >> Reply to this email directly or view it on GitHub\n> > >> <\n> https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-123043167\n> >\n> > >> .\n> > >>\n> > >\n> > >\n> > —\n> > Reply to this email directly or view it on GitHub <\n> https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124252178\n> >.\n> >\n>\n> —\n> Reply to this email directly or view it on GitHub\n> <https://github.com/RobotLocomotion/drake/issues/1193#issuecomment-124255492>\n> .\n>\n"}}