{"action":"created","comment":{"url":"https://api.github.com/repos/RobotLocomotion/drake/pulls/comments/26971036","id":26971036,"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();\n+  const int nP = phiP.size();","path":"systems/plants/setupLCPmex.cpp","position":132,"original_position":132,"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":"warning on MSVC?","created_at":"2015-03-23T19:35:01Z","updated_at":"2015-03-23T19:35:01Z","html_url":"https://github.com/RobotLocomotion/drake/pull/882#discussion_r26971036","pull_request_url":"https://api.github.com/repos/RobotLocomotion/drake/pulls/882","_links":{"self":{"href":"https://api.github.com/repos/RobotLocomotion/drake/pulls/comments/26971036"},"html":{"href":"https://github.com/RobotLocomotion/drake/pull/882#discussion_r26971036"},"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:35:01Z","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"}}}}