{"action":"created","comment":{"url":"https://api.github.com/repos/RobotLocomotion/drake/pulls/comments/26970997","id":26970997,"diff_hunk":"@@ -0,0 +1,275 @@\n+#include \"mex.h\"\n+#include <iostream>\n+#include \"drakeUtil.h\"\n+#include \"RigidBodyManipulator.h\"\n+#include \"math.h\"\n+#include <sstream>\n+\n+using namespace Eigen;\n+using namespace std;\n+\n+template <typename DerivedA, typename DerivedB, typename DerivedC, typename DerivedD> \n+inline void manipulatorDynamics(RigidBodyManipulator *model, MatrixBase<DerivedA> const & q, MatrixBase<DerivedA> const & v, MatrixBase<DerivedB> &H, MatrixBase<DerivedC> &C, MatrixBase<DerivedD> &B)\n+{\n+  const int nq = model->num_positions;\n+  H.resize(nq,nq); \n+  C = VectorXd::Zero(nq);\n+  B = model->B;\n+  model->HandC(q, v, (MatrixXd*)nullptr, H, C, (MatrixXd*)nullptr, (MatrixXd*)nullptr, (MatrixXd*)nullptr);\n+}\n+\n+inline void getInclusionIndices(vector<bool> const & inclusion, vector<int> & indices, bool getTrueIndices)\n+{\n+  const int n = inclusion.size();\n+  indices.clear();\n+  for (int x = 0; x < n; x++) {\n+    if(inclusion[x] == getTrueIndices){\n+      indices.push_back(x);\n+    }\n+  }\n+}\n+\n+template <typename Derived>\n+inline void getThresholdInclusion(MatrixBase<Derived> const &values, const double threshold, vector<bool> & belowThreshold)\n+{\n+  const int n = values.size();\n+  belowThreshold.clear();\n+  for (int x = 0; x < n; x++) {\n+    belowThreshold.push_back(values[x]<threshold);\n+  }\n+}\n+\n+//counts number of inclusions\n+inline int getNumTrue(vector<bool> const & bools)\n+{\n+  int count = 0;\n+  const int n = bools.size();\n+  for (int x = 0; x < n; x++) {\n+    if (bools[x]) {\n+      count++;\n+    }\n+  }\n+  return count;\n+}\n+\n+//splits a vector into two based on inclusion mapping\n+inline void partitionVector(vector<bool> const &indices, VectorXd const & v, VectorXd & included, VectorXd & excluded)\n+{\n+  const int n = indices.size();\n+  const int count = getNumTrue(indices);\n+\n+  included = VectorXd::Zero(count);\n+  excluded = VectorXd::Zero(n-count);\n+  \n+  int inclusionIndex = 0;\n+  int exclusionIndex = 0;\n+\n+  for (int x = 0; x < n; x++) {\n+    if (indices[x]) {\n+      included[inclusionIndex++] = v[x];\n+    } else {\n+      excluded[exclusionIndex++] = v[x];\n+    }\n+  }\n+}\n+\n+//splits a matrix into two based on a row inclusion mapping\n+inline void partitionMatrix(vector<bool> const &indices, MatrixXd const & M, MatrixXd & included, MatrixXd & excluded)\n+{   \n+  const int n = indices.size();\n+  const int count = getNumTrue(indices);\n+  const int cols = M.cols();\n+\n+  included = MatrixXd::Zero(count, cols);\n+  excluded = MatrixXd::Zero(n-count, cols);\n+\n+  int inclusionIndex = 0;\n+  int exclusionIndex = 0;\n+\n+  for (int x = 0; x < n; x++) {\n+    if (indices[x]) {\n+      included.row(inclusionIndex++) = M.row(x);\n+    } else {\n+      excluded.row(exclusionIndex++) = M.row(x);\n+    }\n+  }\n+}\n+\n+//builds a filtered matrix containing only the rows specified by indices\n+inline void filterByIndices(vector<int> const &indices, MatrixXd const & M, MatrixXd & filtered)\n+{\n+  const int n = indices.size();\n+  filtered = MatrixXd::Zero(n, M.cols());\n+  for (int x = 0; x < n; x++) {\n+    filtered.row(x) = M.row(indices[x]);\n+  }\n+}\n+\n+//[M, w, Mqdn, wqdn] = setupLCPmex(mex_model_ptr, q, qd, u, phiC, n, D, h, z_inactive_guess_tol)\n+void mexFunction( int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[] ) { \n+  RigidBodyManipulator *model= (RigidBodyManipulator*) getDrakeMexPointer(prhs[0]);\n+  const int nq = model->num_positions;\n+  const int nv = model->num_velocities;\n+  const int numContactPairs = mxGetNumberOfElements(prhs[4]);\n+  const int mC = mxGetNumberOfElements(prhs[6]);\n+  const double z_inactive_guess_tol = static_cast<double>(mxGetScalar(prhs[8]));\n+  const double h = static_cast<double>(mxGetScalar(prhs[7]));\n+  const Map<VectorXd> q(mxGetPr(prhs[1]),nq);\n+  const Map<VectorXd> v(mxGetPr(prhs[2]),nv);\n+  const Map<VectorXd> u(mxGetPr(prhs[3]),model->B.cols());\n+  \n+  const Map<VectorXd> phiC(mxGetPr(prhs[4]),numContactPairs);\n+  const Map<MatrixXd> n(mxGetPr(prhs[5]), numContactPairs, nq);\n+\n+  VectorXd C, phiL, phiP, phiL_possible, phiC_possible, phiL_check, phiC_check;\n+  MatrixXd H, B, JP, JL, JL_possible, n_possible, JL_check, n_check;\n+\n+  manipulatorDynamics(model, q, v, H, C, B);\n+\n+  model->positionConstraints(phiP, JP);\n+  model->jointLimitConstraints(q, phiL, JL);\n+  MatrixXd Hinv = H.inverse();","path":"systems/plants/setupLCPmex.cpp","position":131,"original_position":131,"commit_id":"8e86a0290f6783ef76ee156274096897a8ebdb41","original_commit_id":"8e86a0290f6783ef76ee156274096897a8ebdb41","user":{"login":"tkoolen","id":2585880,"avatar_url":"https://avatars.githubusercontent.com/u/2585880?v=3","gravatar_id":"","url":"https://api.github.com/users/tkoolen","html_url":"https://github.com/tkoolen","followers_url":"https://api.github.com/users/tkoolen/followers","following_url":"https://api.github.com/users/tkoolen/following{/other_user}","gists_url":"https://api.github.com/users/tkoolen/gists{/gist_id}","starred_url":"https://api.github.com/users/tkoolen/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/tkoolen/subscriptions","organizations_url":"https://api.github.com/users/tkoolen/orgs","repos_url":"https://api.github.com/users/tkoolen/repos","events_url":"https://api.github.com/users/tkoolen/events{/privacy}","received_events_url":"https://api.github.com/users/tkoolen/received_events","type":"User","site_admin":false},"body":"I think using a solve may be faster, even if you're making use of Hinv in a number of place. Should probably use a decomposition that exploits the fact that H is positive definite, such as Cholesky (LLT): \r\nhttp://eigen.tuxfamily.org/dox/classEigen_1_1LLT.html\r\n","created_at":"2015-03-23T19:34:37Z","updated_at":"2015-03-23T19:34:37Z","html_url":"https://github.com/RobotLocomotion/drake/pull/882#discussion_r26970997","pull_request_url":"https://api.github.com/repos/RobotLocomotion/drake/pulls/882","_links":{"self":{"href":"https://api.github.com/repos/RobotLocomotion/drake/pulls/comments/26970997"},"html":{"href":"https://github.com/RobotLocomotion/drake/pull/882#discussion_r26970997"},"pull_request":{"href":"https://api.github.com/repos/RobotLocomotion/drake/pulls/882"}}},"pull_request":{"url":"https://api.github.com/repos/RobotLocomotion/drake/pulls/882","id":31648844,"html_url":"https://github.com/RobotLocomotion/drake/pull/882","diff_url":"https://github.com/RobotLocomotion/drake/pull/882.diff","patch_url":"https://github.com/RobotLocomotion/drake/pull/882.patch","issue_url":"https://api.github.com/repos/RobotLocomotion/drake/issues/882","number":882,"state":"open","locked":false,"title":"SolveLCP in C++","user":{"login":"psiorx","id":3849600,"avatar_url":"https://avatars.githubusercontent.com/u/3849600?v=3","gravatar_id":"","url":"https://api.github.com/users/psiorx","html_url":"https://github.com/psiorx","followers_url":"https://api.github.com/users/psiorx/followers","following_url":"https://api.github.com/users/psiorx/following{/other_user}","gists_url":"https://api.github.com/users/psiorx/gists{/gist_id}","starred_url":"https://api.github.com/users/psiorx/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/psiorx/subscriptions","organizations_url":"https://api.github.com/users/psiorx/orgs","repos_url":"https://api.github.com/users/psiorx/repos","events_url":"https://api.github.com/users/psiorx/events{/privacy}","received_events_url":"https://api.github.com/users/psiorx/received_events","type":"User","site_admin":false},"body":"This started as just setting up the LCP problem in mex, but it turned into a full solveLCP replacement. Currently, everything is in mex except for the call to contactConstraints because the portion of collisionDetect which handles terrain contact is still in MATLAB.\r\n\r\nThis now works with the current kinsol.  It currently only supports relative position constraints (loop joints) since cables are not yet implemented in c++.\r\n\r\nThere is still a lot of room for improvement since the actual call to pathlcp is being invoked using mexCallMATLAB. This is because only 32-bit C++ libs are available for path (I've gotten into contact with Michael Ferris about supplying us with 64-bit C++ libraries for all platforms).\r\n\r\nThe other obvious improvement is including the call to fastQP directly in C++ to avoid calling path when M is convex.\r\n\r\nEven without these optimizations, this is about twice as fast as the current solveLCP in atlas tests with a hand attached, with the setup portion being 15-20x faster. ","created_at":"2015-03-20T20:25:26Z","updated_at":"2015-03-23T19:34:37Z","closed_at":null,"merged_at":null,"merge_commit_sha":"f2bdd53b0396b960457c1676dc9d7763f2d0563a","assignee":{"login":"RussTedrake","id":6442292,"avatar_url":"https://avatars.githubusercontent.com/u/6442292?v=3","gravatar_id":"","url":"https://api.github.com/users/RussTedrake","html_url":"https://github.com/RussTedrake","followers_url":"https://api.github.com/users/RussTedrake/followers","following_url":"https://api.github.com/users/RussTedrake/following{/other_user}","gists_url":"https://api.github.com/users/RussTedrake/gists{/gist_id}","starred_url":"https://api.github.com/users/RussTedrake/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/RussTedrake/subscriptions","organizations_url":"https://api.github.com/users/RussTedrake/orgs","repos_url":"https://api.github.com/users/RussTedrake/repos","events_url":"https://api.github.com/users/RussTedrake/events{/privacy}","received_events_url":"https://api.github.com/users/RussTedrake/received_events","type":"User","site_admin":false},"milestone":null,"commits_url":"https://api.github.com/repos/RobotLocomotion/drake/pulls/882/commits","review_comments_url":"https://api.github.com/repos/RobotLocomotion/drake/pulls/882/comments","review_comment_url":"https://api.github.com/repos/RobotLocomotion/drake/pulls/comments{/number}","comments_url":"https://api.github.com/repos/RobotLocomotion/drake/issues/882/comments","statuses_url":"https://api.github.com/repos/RobotLocomotion/drake/statuses/8e86a0290f6783ef76ee156274096897a8ebdb41","head":{"label":"psiorx:jc-setupLCPmex","ref":"jc-setupLCPmex","sha":"8e86a0290f6783ef76ee156274096897a8ebdb41","user":{"login":"psiorx","id":3849600,"avatar_url":"https://avatars.githubusercontent.com/u/3849600?v=3","gravatar_id":"","url":"https://api.github.com/users/psiorx","html_url":"https://github.com/psiorx","followers_url":"https://api.github.com/users/psiorx/followers","following_url":"https://api.github.com/users/psiorx/following{/other_user}","gists_url":"https://api.github.com/users/psiorx/gists{/gist_id}","starred_url":"https://api.github.com/users/psiorx/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/psiorx/subscriptions","organizations_url":"https://api.github.com/users/psiorx/orgs","repos_url":"https://api.github.com/users/psiorx/repos","events_url":"https://api.github.com/users/psiorx/events{/privacy}","received_events_url":"https://api.github.com/users/psiorx/received_events","type":"User","site_admin":false},"repo":{"id":30214282,"name":"drake","full_name":"psiorx/drake","owner":{"login":"psiorx","id":3849600,"avatar_url":"https://avatars.githubusercontent.com/u/3849600?v=3","gravatar_id":"","url":"https://api.github.com/users/psiorx","html_url":"https://github.com/psiorx","followers_url":"https://api.github.com/users/psiorx/followers","following_url":"https://api.github.com/users/psiorx/following{/other_user}","gists_url":"https://api.github.com/users/psiorx/gists{/gist_id}","starred_url":"https://api.github.com/users/psiorx/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/psiorx/subscriptions","organizations_url":"https://api.github.com/users/psiorx/orgs","repos_url":"https://api.github.com/users/psiorx/repos","events_url":"https://api.github.com/users/psiorx/events{/privacy}","received_events_url":"https://api.github.com/users/psiorx/received_events","type":"User","site_admin":false},"private":false,"html_url":"https://github.com/psiorx/drake","description":"A planning, control, and analysis toolbox for nonlinear dynamical systems.  Please see the WIKI for documentation.","fork":true,"url":"https://api.github.com/repos/psiorx/drake","forks_url":"https://api.github.com/repos/psiorx/drake/forks","keys_url":"https://api.github.com/repos/psiorx/drake/keys{/key_id}","collaborators_url":"https://api.github.com/repos/psiorx/drake/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/psiorx/drake/teams","hooks_url":"https://api.github.com/repos/psiorx/drake/hooks","issue_events_url":"https://api.github.com/repos/psiorx/drake/issues/events{/number}","events_url":"https://api.github.com/repos/psiorx/drake/events","assignees_url":"https://api.github.com/repos/psiorx/drake/assignees{/user}","branches_url":"https://api.github.com/repos/psiorx/drake/branches{/branch}","tags_url":"https://api.github.com/repos/psiorx/drake/tags","blobs_url":"https://api.github.com/repos/psiorx/drake/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/psiorx/drake/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/psiorx/drake/git/refs{/sha}","trees_url":"https://api.github.com/repos/psiorx/drake/git/trees{/sha}","statuses_url":"https://api.github.com/repos/psiorx/drake/statuses/{sha}","languages_url":"https://api.github.com/repos/psiorx/drake/languages","stargazers_url":"https://api.github.com/repos/psiorx/drake/stargazers","contributors_url":"https://api.github.com/repos/psiorx/drake/contributors","subscribers_url":"https://api.github.com/repos/psiorx/drake/subscribers","subscription_url":"https://api.github.com/repos/psiorx/drake/subscription","commits_url":"https://api.github.com/repos/psiorx/drake/commits{/sha}","git_commits_url":"https://api.github.com/repos/psiorx/drake/git/commits{/sha}","comments_url":"https://api.github.com/repos/psiorx/drake/comments{/number}","issue_comment_url":"https://api.github.com/repos/psiorx/drake/issues/comments{/number}","contents_url":"https://api.github.com/repos/psiorx/drake/contents/{+path}","compare_url":"https://api.github.com/repos/psiorx/drake/compare/{base}...{head}","merges_url":"https://api.github.com/repos/psiorx/drake/merges","archive_url":"https://api.github.com/repos/psiorx/drake/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/psiorx/drake/downloads","issues_url":"https://api.github.com/repos/psiorx/drake/issues{/number}","pulls_url":"https://api.github.com/repos/psiorx/drake/pulls{/number}","milestones_url":"https://api.github.com/repos/psiorx/drake/milestones{/number}","notifications_url":"https://api.github.com/repos/psiorx/drake/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/psiorx/drake/labels{/name}","releases_url":"https://api.github.com/repos/psiorx/drake/releases{/id}","created_at":"2015-02-02T23:23:34Z","updated_at":"2015-02-13T16:18:50Z","pushed_at":"2015-03-21T18:05:56Z","git_url":"git://github.com/psiorx/drake.git","ssh_url":"git@github.com:psiorx/drake.git","clone_url":"https://github.com/psiorx/drake.git","svn_url":"https://github.com/psiorx/drake","homepage":"https://github.com/RobotLocomotion/drake/wiki","size":147791,"stargazers_count":0,"watchers_count":0,"language":"Matlab","has_issues":false,"has_downloads":true,"has_wiki":true,"has_pages":false,"forks_count":0,"mirror_url":null,"open_issues_count":0,"forks":0,"open_issues":0,"watchers":0,"default_branch":"master"}},"base":{"label":"RobotLocomotion:master","ref":"master","sha":"bea986a48e91c3c5d2a97807df5d0f2f3bd34396","user":{"login":"RobotLocomotion","id":6496345,"avatar_url":"https://avatars.githubusercontent.com/u/6496345?v=3","gravatar_id":"","url":"https://api.github.com/users/RobotLocomotion","html_url":"https://github.com/RobotLocomotion","followers_url":"https://api.github.com/users/RobotLocomotion/followers","following_url":"https://api.github.com/users/RobotLocomotion/following{/other_user}","gists_url":"https://api.github.com/users/RobotLocomotion/gists{/gist_id}","starred_url":"https://api.github.com/users/RobotLocomotion/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/RobotLocomotion/subscriptions","organizations_url":"https://api.github.com/users/RobotLocomotion/orgs","repos_url":"https://api.github.com/users/RobotLocomotion/repos","events_url":"https://api.github.com/users/RobotLocomotion/events{/privacy}","received_events_url":"https://api.github.com/users/RobotLocomotion/received_events","type":"Organization","site_admin":false},"repo":{"id":16256144,"name":"drake","full_name":"RobotLocomotion/drake","owner":{"login":"RobotLocomotion","id":6496345,"avatar_url":"https://avatars.githubusercontent.com/u/6496345?v=3","gravatar_id":"","url":"https://api.github.com/users/RobotLocomotion","html_url":"https://github.com/RobotLocomotion","followers_url":"https://api.github.com/users/RobotLocomotion/followers","following_url":"https://api.github.com/users/RobotLocomotion/following{/other_user}","gists_url":"https://api.github.com/users/RobotLocomotion/gists{/gist_id}","starred_url":"https://api.github.com/users/RobotLocomotion/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/RobotLocomotion/subscriptions","organizations_url":"https://api.github.com/users/RobotLocomotion/orgs","repos_url":"https://api.github.com/users/RobotLocomotion/repos","events_url":"https://api.github.com/users/RobotLocomotion/events{/privacy}","received_events_url":"https://api.github.com/users/RobotLocomotion/received_events","type":"Organization","site_admin":false},"private":false,"html_url":"https://github.com/RobotLocomotion/drake","description":"A planning, control, and analysis toolbox for nonlinear dynamical systems.  Please see the WIKI for documentation.","fork":false,"url":"https://api.github.com/repos/RobotLocomotion/drake","forks_url":"https://api.github.com/repos/RobotLocomotion/drake/forks","keys_url":"https://api.github.com/repos/RobotLocomotion/drake/keys{/key_id}","collaborators_url":"https://api.github.com/repos/RobotLocomotion/drake/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/RobotLocomotion/drake/teams","hooks_url":"https://api.github.com/repos/RobotLocomotion/drake/hooks","issue_events_url":"https://api.github.com/repos/RobotLocomotion/drake/issues/events{/number}","events_url":"https://api.github.com/repos/RobotLocomotion/drake/events","assignees_url":"https://api.github.com/repos/RobotLocomotion/drake/assignees{/user}","branches_url":"https://api.github.com/repos/RobotLocomotion/drake/branches{/branch}","tags_url":"https://api.github.com/repos/RobotLocomotion/drake/tags","blobs_url":"https://api.github.com/repos/RobotLocomotion/drake/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/RobotLocomotion/drake/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/RobotLocomotion/drake/git/refs{/sha}","trees_url":"https://api.github.com/repos/RobotLocomotion/drake/git/trees{/sha}","statuses_url":"https://api.github.com/repos/RobotLocomotion/drake/statuses/{sha}","languages_url":"https://api.github.com/repos/RobotLocomotion/drake/languages","stargazers_url":"https://api.github.com/repos/RobotLocomotion/drake/stargazers","contributors_url":"https://api.github.com/repos/RobotLocomotion/drake/contributors","subscribers_url":"https://api.github.com/repos/RobotLocomotion/drake/subscribers","subscription_url":"https://api.github.com/repos/RobotLocomotion/drake/subscription","commits_url":"https://api.github.com/repos/RobotLocomotion/drake/commits{/sha}","git_commits_url":"https://api.github.com/repos/RobotLocomotion/drake/git/commits{/sha}","comments_url":"https://api.github.com/repos/RobotLocomotion/drake/comments{/number}","issue_comment_url":"https://api.github.com/repos/RobotLocomotion/drake/issues/comments{/number}","contents_url":"https://api.github.com/repos/RobotLocomotion/drake/contents/{+path}","compare_url":"https://api.github.com/repos/RobotLocomotion/drake/compare/{base}...{head}","merges_url":"https://api.github.com/repos/RobotLocomotion/drake/merges","archive_url":"https://api.github.com/repos/RobotLocomotion/drake/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/RobotLocomotion/drake/downloads","issues_url":"https://api.github.com/repos/RobotLocomotion/drake/issues{/number}","pulls_url":"https://api.github.com/repos/RobotLocomotion/drake/pulls{/number}","milestones_url":"https://api.github.com/repos/RobotLocomotion/drake/milestones{/number}","notifications_url":"https://api.github.com/repos/RobotLocomotion/drake/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/RobotLocomotion/drake/labels{/name}","releases_url":"https://api.github.com/repos/RobotLocomotion/drake/releases{/id}","created_at":"2014-01-26T16:11:05Z","updated_at":"2015-03-23T11:00:36Z","pushed_at":"2015-03-23T19:03:51Z","git_url":"git://github.com/RobotLocomotion/drake.git","ssh_url":"git@github.com:RobotLocomotion/drake.git","clone_url":"https://github.com/RobotLocomotion/drake.git","svn_url":"https://github.com/RobotLocomotion/drake","homepage":"https://github.com/RobotLocomotion/drake/wiki","size":174667,"stargazers_count":100,"watchers_count":100,"language":"Matlab","has_issues":true,"has_downloads":true,"has_wiki":true,"has_pages":false,"forks_count":93,"mirror_url":null,"open_issues_count":110,"forks":93,"open_issues":110,"watchers":100,"default_branch":"master"}},"_links":{"self":{"href":"https://api.github.com/repos/RobotLocomotion/drake/pulls/882"},"html":{"href":"https://github.com/RobotLocomotion/drake/pull/882"},"issue":{"href":"https://api.github.com/repos/RobotLocomotion/drake/issues/882"},"comments":{"href":"https://api.github.com/repos/RobotLocomotion/drake/issues/882/comments"},"review_comments":{"href":"https://api.github.com/repos/RobotLocomotion/drake/pulls/882/comments"},"review_comment":{"href":"https://api.github.com/repos/RobotLocomotion/drake/pulls/comments{/number}"},"commits":{"href":"https://api.github.com/repos/RobotLocomotion/drake/pulls/882/commits"},"statuses":{"href":"https://api.github.com/repos/RobotLocomotion/drake/statuses/8e86a0290f6783ef76ee156274096897a8ebdb41"}}}}