ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

# Revision history [back]

Try this:

/* Adapted from pypose: * Simple 3dof leg solver. X,Y,Z are the length from the Coxa rotate to the endpoint. */

ikSolution_t calcLegIK(s32 X, s32 Y, s32 Z) {

ikSolution_t ans;
s32 trueX;
s32 im;
float q1, q2;
s32 d1, d2;

// first, make this a 2DOF problem... by solving coxa
trueX = sqrt(X*X + Y*Y) - COXA_LENGTH;
im = sqrt(trueX*trueX + Z*Z);    // length of imaginary leg

// get femur angle above horizon...
q1 = atan2(Z, trueX);
d1 = (FEMUR_LENGTH*FEMUR_LENGTH) - (TIBIA_LENGTH*TIBIA_LENGTH) + (im*im);
d2 = 2 * FEMUR_LENGTH * im;
q2 = acos((float)d1/(float)d2);

// and tibia angle from femur...
d1 = (FEMUR_LENGTH*FEMUR_LENGTH) - (im*im) + (TIBIA_LENGTH*TIBIA_LENGTH);
d2 = 2* TIBIA_LENGTH * FEMUR_LENGTH;

return ans;


}

HTH

Try this:

 /* Adapted from pypose:
* Simple 3dof leg solver. X,Y,Z are the length from the Coxa rotate to the endpoint.
*/ */

ikSolution_t calcLegIK(s32 X, s32 Y, s32 Z)
{ {

ikSolution_t ans;
s32 trueX;
s32 im;
float q1, q2;
s32 d1, d2;

// first, make this a 2DOF problem... by solving coxa
trueX = sqrt(X*X + Y*Y) - COXA_LENGTH;
im = sqrt(trueX*trueX + Z*Z);    // length of imaginary leg

// get femur angle above horizon...
q1 = atan2(Z, trueX);
d1 = (FEMUR_LENGTH*FEMUR_LENGTH) - (TIBIA_LENGTH*TIBIA_LENGTH) + (im*im);
d2 = 2 * FEMUR_LENGTH * im;
q2 = acos((float)d1/(float)d2);

// and tibia angle from femur...
d1 = (FEMUR_LENGTH*FEMUR_LENGTH) - (im*im) + (TIBIA_LENGTH*TIBIA_LENGTH);
d2 = 2* TIBIA_LENGTH * FEMUR_LENGTH;
} HTH 




 ROS Answers is licensed under Creative Commons Attribution 3.0 Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license. about | faq | help | privacy policy | terms of service Powered by Askbot version 0.10.2 Please note: ROS Answers requires javascript to work properly, please enable javascript in your browser, here is how //IE fix to hide the red margin var noscript = document.getElementsByTagName('noscript')[0]; noscript.style.padding = '0px'; noscript.style.backgroundColor = 'transparent'; askbot['urls']['mark_read_message'] = '/s/messages/markread/'; askbot['urls']['get_tags_by_wildcard'] = '/s/get-tags-by-wildcard/'; askbot['urls']['get_tag_list'] = '/s/get-tag-list/'; askbot['urls']['follow_user'] = '/followit/follow/user/{{userId}}/'; askbot['urls']['unfollow_user'] = '/followit/unfollow/user/{{userId}}/'; askbot['urls']['user_signin'] = '/account/signin/'; askbot['urls']['getEditor'] = '/s/get-editor/'; askbot['urls']['apiGetQuestions'] = '/s/api/get_questions/'; askbot['urls']['ask'] = '/questions/ask/'; askbot['urls']['questions'] = '/questions/'; askbot['settings']['groupsEnabled'] = false; askbot['settings']['static_url'] = '/m/'; askbot['settings']['minSearchWordLength'] = 3; askbot['settings']['mathjaxEnabled'] = false; askbot['settings']['sharingSuffixText'] = ''; askbot['settings']['errorPlacement'] = 'after-label'; askbot['data']['maxCommentLength'] = 1000; askbot['settings']['editorType'] = 'markdown'; askbot['settings']['commentsEditorType'] = 'rich\u002Dtext'; askbot['messages']['askYourQuestion'] = 'Ask Your Question'; askbot['messages']['acceptOwnAnswer'] = 'accept or unaccept your own answer'; askbot['messages']['followQuestions'] = 'follow questions'; askbot['settings']['allowedUploadFileTypes'] = [ "jpg", "jpeg", "gif", "bmp", "png", "tiff" ]; askbot['data']['haveFlashNotifications'] = true; askbot['data']['activeTab'] = 'questions'; askbot['settings']['csrfCookieName'] = 'csrftoken'; askbot['data']['searchUrl'] = ''; /*<![CDATA[*/ $('.mceStatusbar').remove();//a hack to remove the tinyMCE status bar$(document).ready(function(){ // focus input on the search bar endcomment var activeTab = askbot['data']['activeTab']; if (inArray(activeTab, ['users', 'questions', 'tags', 'badges'])) { var searchInput = $('#keywords'); } else if (activeTab === 'ask') { var searchInput =$('#id_title'); } else { var searchInput = undefined; animateHashes(); } var wasScrolled = $('#scroll-mem').val(); if (searchInput && !wasScrolled) { searchInput.focus(); putCursorAtEnd(searchInput); } var haveFullTextSearchTab = inArray(activeTab, ['questions', 'badges', 'ask']); var haveUserProfilePage =$('body').hasClass('user-profile-page'); if ((haveUserProfilePage || haveFullTextSearchTab) && searchInput && searchInput.length) { var search = new FullTextSearch(); askbot['controllers'] = askbot['controllers'] || {}; askbot['controllers']['fullTextSearch'] = search; search.setSearchUrl(askbot['data']['searchUrl']); if (activeTab === 'ask') { search.setAskButtonEnabled(false); } search.decorate(searchInput); } else if (activeTab === 'tags') { var search = new TagSearch(); search.decorate(searchInput); } if (askbot['data']['userIsAdminOrMod']) { $('body').addClass('admin'); } if (askbot['settings']['groupsEnabled']) { askbot['urls']['add_group'] = "/s/add-group/"; var group_dropdown = new GroupDropdown();$('.groups-dropdown').append(group_dropdown.getElement()); } var userRep = $('#userToolsNav .reputation'); if (userRep.length) { var showPermsTrigger = new ShowPermsTrigger(); showPermsTrigger.decorate(userRep); } }); if (askbot['data']['haveFlashNotifications']) {$('#validate_email_alert').click(function(){notify.close(true)}) notify.show(); } var langNav = $('.lang-nav'); if (langNav.length) { var nav = new LangNav(); nav.decorate(langNav); } /*]]>*/ var gaJsHost = (("https:" == document.location.protocol) ? "https://ssl." : "http://www."); document.write(unescape("%3Cscript src='" + gaJsHost + "google-analytics.com/ga.js' type='text/javascript'%3E%3C/script%3E")); try { var pageTracker = _gat._getTracker('UA-17821189-3'); pageTracker._trackPageview(); } catch(err) {} //todo - take this out into .js file$(document).ready(function(){ $('div.revision div[id^=rev-header-]').bind('click', function(){ var revId = this.id.substr(11); toggleRev(revId); }); lanai.highlightSyntax(); }); function toggleRev(id) { var arrow =$("#rev-arrow-" + id); var visible = arrow.attr("src").indexOf("hide") > -1; if (visible) { var image_path = '/m/default/media/images/expander-arrow-show.gif?v=28'; } else { var image_path = '/m/default/media/images/expander-arrow-hide.gif?v=28'; } image_path = image_path + "?v=28"; arrow.attr("src", image_path); \$("#rev-body-" + id).slideToggle("fast"); } for (url_name in askbot['urls']){ askbot['urls'][url_name] = cleanUrl(askbot['urls'][url_name]); }