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;
}
