void RobotKinematics3D::GetJacobianDeriv_Fast(const Vector3& pm, int m, int i, int j, Vector3&ddtheta,Vector3& ddp) const { Assert(i <= m); Assert(j <= m); bool swapped = false; if(i < j) { //order so j < i, Swap(i,j); swapped = true; } //Get i's Jacobian then transform to j, and modify Frame3D JPmi,JPjj; Matrix3 R0_j; links[i].GetJacobian(q[i],links[m].T_World,JPmi); links[j].GetJacobian(q[j],links[j].T_World,JPjj); R0_j.setTranspose(links[j].T_World.R); Matrix3 RA1B = JPjj.R*R0_j; ddp = RA1B*(JPmi*pm); if(swapped) ddtheta.setZero(); else ddtheta=RA1B*links[i].T_World.R*links[i].w; }
//--------------------------------------------------------------------------- void NodeSelector::centerOfSelection( Vector3& centerOfSelection ) { if (selectionList_.size( ) == 0) return; centerOfSelection.setZero( ); SceneNodeVector::iterator it = selectionList_.begin( ); SceneNodeVector::iterator end = selectionList_.end( ); while ( it != end ) { SceneNode *node = (*it); //accumulate location /*BoundingBox bb = node->boundingBox( ); //transfrom bounding box to world coords bb.transformBy( node->worldTransform( ) ); Vector3 centerOfNode = ( bb.minBounds() + bb.maxBounds() ) / 2.f; centerOfSelection += centerOfNode;*/ centerOfSelection += node->worldTransform( ).applyToOrigin(); it++; } // the height for center of selection is the height of the origin of the // last node in world space or what is in effect the current floor height //if ( selectionList_.size( ) ) // centerOfSelection[1] = (*(end-1))->worldTransform()[3][1]; //size of vector is guaranteed not to be zero centerOfSelection /= selectionList_.size( ); }
template<typename Scalar> void eulerangles() { typedef Matrix<Scalar,3,3> Matrix3; typedef Matrix<Scalar,3,1> Vector3; typedef Array<Scalar,3,1> Array3; typedef Quaternion<Scalar> Quaternionx; typedef AngleAxis<Scalar> AngleAxisx; Scalar a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI)); Quaternionx q1; q1 = AngleAxisx(a, Vector3::Random().normalized()); Matrix3 m; m = q1; Vector3 ea = m.eulerAngles(0,1,2); check_all_var(ea); ea = m.eulerAngles(0,1,0); check_all_var(ea); ea = (Array3::Random() + Array3(1,1,0))*Scalar(M_PI)*Array3(0.5,0.5,1); check_all_var(ea); ea[2] = ea[0] = internal::random<Scalar>(0,Scalar(M_PI)); check_all_var(ea); ea[0] = ea[1] = internal::random<Scalar>(0,Scalar(M_PI)); check_all_var(ea); ea[1] = 0; check_all_var(ea); ea.head(2).setZero(); check_all_var(ea); ea.setZero(); check_all_var(ea); }
virtual bool Initialize() { cur_link=0; cur_driver=0; link_value = 0; driver_value = 0; draw_geom = 1; draw_bbs = 0; draw_com = 0; draw_frame = 0; draw_expanded = 0; draw_self_collision_tests = 0; hoverLink=hoverWidget=-1; hoverPt.setZero(); pose_ik = 0; if(!RobotViewProgram::Initialize()) return false; self_colliding.resize(robot->links.size(),false); UpdateConfig(); /* //TEST: robot-to-robot IK test. only works for AL5Dx2 IKGoal test; test.link = 8; test.destLink = 16; test.localPosition.set(0,0,0.05); test.endPosition.set(0,0,0.05); //test.SetFixedPosition(test.endPosition); Matrix3 R; R.setRotateZ(120); test.SetFixedRotation(R); vector<IKGoal> problem(1,test); int iters=100; bool res=SolveIK(*robot,problem,1e-3,iters); printf("Solved IK: %d, %d iters, error %g\n",(int)res,iters,RobotIKError(*robot,test)); UpdateConfig(); */ //setup GUI glui = GLUI_Master.create_glui_subwindow(main_window,GLUI_SUBWINDOW_RIGHT); glui->set_main_gfx_window(main_window); GLUI_Panel* panel = glui->add_panel("Link controls"); link_spinner = glui->add_spinner_to_panel(panel,"Index",GLUI_SPINNER_INT,&cur_link,LINK_SPINNER_ID,ControlFunc); link_spinner->set_int_limits(0,robot->links.size()-1,GLUI_LIMIT_WRAP); link_listbox = glui->add_listbox_to_panel(panel,"Name",&cur_link,LINK_LISTBOX_ID,ControlFunc); for(size_t i=0;i<robot->links.size();i++) { char buf[256]; strcpy(buf,robot->linkNames[i].c_str()); link_listbox->add_item(i,buf); } link_value_spinner = glui->add_spinner_to_panel(panel,"Angle",GLUI_SPINNER_FLOAT,&link_value,LINK_VALUE_SPINNER_ID,ControlFunc); link_info = glui->add_statictext_to_panel(panel,"Info"); UpdateLinkValueGUI(); UpdateLinkInfoGUI(); panel = glui->add_panel("Driver controls"); glui->add_checkbox_to_panel(panel,"Pose by IK",&pose_ik); driver_spinner = glui->add_spinner_to_panel(panel,"Index",GLUI_SPINNER_INT,&cur_driver,DRIVER_SPINNER_ID,ControlFunc); driver_spinner->set_int_limits(0,(int)robot->drivers.size()-1,GLUI_LIMIT_WRAP); driver_listbox = glui->add_listbox_to_panel(panel,"Name",&cur_driver,DRIVER_LISTBOX_ID,ControlFunc); for(size_t i=0;i<robot->drivers.size();i++) { char buf[256]; strcpy(buf,robot->driverNames[i].c_str()); driver_listbox->add_item(i,buf); } driver_value_spinner = glui->add_spinner_to_panel(panel,"Angle",GLUI_SPINNER_FLOAT,&driver_value,DRIVER_VALUE_SPINNER_ID,ControlFunc); driver_info = glui->add_statictext_to_panel(panel,"Info"); glui->add_checkbox("Draw geometry",&draw_geom); glui->add_checkbox("Draw COM",&draw_com); glui->add_checkbox("Draw frame",&draw_frame); glui->add_checkbox("Draw bboxes",&draw_bbs); glui->add_checkbox("Draw expanded",&draw_expanded,DRAW_EXPANDED_CHECKBOX_ID,ControlFunc); glui->add_checkbox("Draw collision tests",&draw_self_collision_tests); UpdateDriverValueGUI(); UpdateDriverInfoGUI(); return true; }