void unigripper_motoman_plant_t::update_phys_configs(config_list_t& configs, unsigned& index) const
{
bool update = true;
VectorNd tmpVec = VectorNd::Zero(3);
VectorNd pos;
Matrix3d orient;
Quaternion quat;
double qw,qx,qy,qz;
for(unsigned i=0;i<simple_rigid_body_names.size();i++)
{
unsigned int id = body_ids[i];//model.GetBodyId(simple_rigid_body_names[i].c_str());
pos = CalcBodyToBaseCoordinates(model,Q,id,tmpVec,update);
update = false;
orient = CalcBodyWorldOrientation(model,Q,id,update);
quat = Quaternion::fromMatrix(orient);
augment_config_list(configs,index);
configs[index].first = config_names[i];
configs[index].second.set_position(pos[0],pos[1],pos[2]);
configs[index].second.set_orientation(quat.x(),quat.y(),quat.z(),quat.w());
if(std::isnan(quat.w()) || std::isnan(quat.x()) || std::isnan(quat.y()) || std::isnan(quat.z()) )
{
uni_convert_matrix_to_quaternion(orient,qx,qy,qz,qw);
configs[index].second.set_orientation(qx,qy,qz,qw);
}
index++;
}
}
void two_link_acrobot_t::update_phys_configs(config_list_t& configs, unsigned& index) const
{
augment_config_list(configs,index);
configs[index].first = config_names[2];
configs[index].second.set_position((length / 2.0) * cos(_theta1 - M_PI / 2), (length / 2.0) * sin(_theta1 - M_PI / 2), 1.5);
configs[index].second.set_orientation(0, 0, sin((_theta1 - M_PI / 2) / 2.0), cos((_theta1 - M_PI / 2) / 2.0));
index++;
augment_config_list(configs,index);
configs[index].first = config_names[1];
configs[index].second.set_position((length) * cos(_theta1 - M_PI / 2)+(length / 2.0) * cos(_theta1 + _theta2 - M_PI / 2),
(length) * sin(_theta1 - M_PI / 2)+(length / 2.0) * sin(_theta1 + _theta2 - M_PI / 2),
1.5);
configs[index].second.set_orientation(0, 0, sin((_theta1 + _theta2 - M_PI / 2) / 2.0), cos((_theta1 + _theta2 - M_PI / 2) / 2.0));
index++;
augment_config_list(configs,index);
configs[index].first = config_names[0];
configs[index].second.set_position((length) * cos(_theta1 - M_PI / 2)+(length) * cos(_theta1 + _theta2 - M_PI / 2),
(length) * sin(_theta1 - M_PI / 2)+(length) * sin(_theta1 + _theta2 - M_PI / 2),
1.5);
configs[index].second.zero();
index++;
}
