// TODO: Could probably make this faster by explicitly using Rodrigues' rotation formula
static void updateOrientation( const int nbodies, const int bdy_num, const VectorXs& q0, const VectorXs& v0, const scalar& dt, VectorXs& q1 )
{
assert( q0.size() == q1.size() );
assert( q0.size() % 12 == 0 );
assert( v0.size() * 2 == q0.size() );
assert( 12 * bdy_num < q0.size() );
assert( 12 * nbodies == q0.size() );
// Extract the axis and amount of rotation
Eigen::AngleAxis<scalar> rotation;
{
Vector3s axis{ v0.segment<3>( 3 * nbodies + 3 * bdy_num ) };
scalar theta{ axis.norm() };
if( theta != 0.0 )
{
axis /= theta;
}
theta *= dt;
rotation.axis() = axis;
rotation.angle() = theta;
}
// Start orientation
const Eigen::Map<const Matrix33sr> Q0{ q0.segment<9>( 3 * nbodies + 9 * bdy_num ).data() };
// Orientation we will update
Eigen::Map<Matrix33sr> Q1{ q1.segment<9>( 3 * nbodies + 9 * bdy_num ).data() };
Q1 = rotation * Q0;
}
bool great_circle::arc::has( const Eigen::Vector3d& p ) const
{
//if( begin_coordinates_ == p || end_coordinates_ == p ) { return true; }
if( equal_(begin_, p) || equal_(end_, p )) { return true; }
//great_circle::arc a( begin_coordinates_, p );
Eigen::AngleAxis< double > a = arc::angle_axis(begin_, p);
return equal_( axis(), a.axis() ) && comma::math::less( a.angle(), angle() );
}
void StaticCylinderRenderer::draw( const StaticCylinder& cylinder ) const
{
glPushAttrib( GL_COLOR );
glPushAttrib( GL_LIGHTING );
glPushAttrib( GL_LINE_WIDTH );
glDisable( GL_LIGHTING );
glColor3d( 0.0, 0.0, 0.0 );
glLineWidth( 1.0 );
const Eigen::Matrix<GLfloat,3,1> center{ cylinder.x().cast<GLfloat>() };
const Eigen::Matrix<GLfloat,3,1> translation{ 0.5 * m_L * cylinder.axis().cast<GLfloat>() };
const Eigen::AngleAxis<GLfloat> rotation{ cylinder.R().cast<GLfloat>() };
const GLfloat r{ static_cast<GLfloat>( cylinder.r() ) };
// Draw the top circle
glPushMatrix();
glTranslatef( center.x() + translation.x(), center.y() + translation.y(), center.z() + translation.z() );
glRotatef( ( 180.0 / MathDefines::PI<GLfloat>() ) * rotation.angle(), rotation.axis().x(), rotation.axis().y(), rotation.axis().z() );
glScalef( r, 1.0, r );
glEnableClientState( GL_VERTEX_ARRAY );
glVertexPointer( 3, GL_FLOAT, 0, m_crds.data() );
glDrawArrays( GL_LINE_LOOP, 0, m_crds.size() / 3 );
glDisableClientState( GL_VERTEX_ARRAY );
glPopMatrix();
// Draw the bottom circle
glPushMatrix();
glTranslatef( center.x() - translation.x(), center.y() - translation.y(), center.z() - translation.z() );
glRotatef( ( 180.0 / MathDefines::PI<GLfloat>() ) * rotation.angle(), rotation.axis().x(), rotation.axis().y(), rotation.axis().z() );
glScalef( r, 1.0, r );
glEnableClientState( GL_VERTEX_ARRAY );
glVertexPointer( 3, GL_FLOAT, 0, m_crds.data() );
glDrawArrays( GL_LINE_LOOP, 0, m_crds.size() / 3 );
glDisableClientState( GL_VERTEX_ARRAY );
glPopMatrix();
// Draw the 'supports'
glPushMatrix();
glTranslatef( center.x(), center.y(), center.z() );
glRotatef( ( 180.0 / MathDefines::PI<GLfloat>() ) * rotation.angle(), rotation.axis().x(), rotation.axis().y(), rotation.axis().z() );
glBegin( GL_LINES );
glVertex3d( 0.0, 0.5 * m_L, r );
glVertex3d( 0.0, -0.5 * m_L, r );
glVertex3d( 0.0, 0.5 * m_L, -r );
glVertex3d( 0.0, -0.5 * m_L, -r );
glVertex3d( r, 0.5 * m_L, 0.0 );
glVertex3d( r, -0.5 * m_L, 0.0 );
glVertex3d( -r, 0.5 * m_L, 0.0 );
glVertex3d( -r, -0.5 * m_L, 0.0 );
glEnd();
glPopMatrix();
glPopAttrib();
glPopAttrib();
glPopAttrib();
}
void Helicoid::rotate(const Eigen::AngleAxis<double> &aa)
{
std::vector<Eigen::Vector3d> tmp = coordinates;
for (int i=0; i<coordinates.size();i++)
{
tmp.at(i) << aa.toRotationMatrix()*coordinates.at(i);
}
coordinates=tmp;
}
bool CartesianGenerator::generateNewVelocityProfiles(RTT::base::PortInterface* portInterface){
m_time_passed = os::TimeService::Instance()->secondsSince(m_time_begin);
geometry_msgs::Pose pose;
cmdCartPose.read(pose);
#if 1
std::cout << "A new pose arrived" << std::endl;
std::cout << "position: " << pose.position.x << " " << pose.position.y << " " << pose.position.z << std::endl;
#endif
m_traject_end.p.x(pose.position.x);
m_traject_end.p.y(pose.position.y);
m_traject_end.p.z(pose.position.z);
m_traject_end.M=Rotation::Quaternion(pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w);
// get current position
geometry_msgs::Pose pose_meas;
m_position_meas_port.read(pose_meas);
m_traject_begin.p.x(pose_meas.position.x);
m_traject_begin.p.y(pose_meas.position.y);
m_traject_begin.p.z(pose_meas.position.z);
m_traject_begin.M=Rotation::Quaternion(pose_meas.orientation.x,pose_meas.orientation.y,pose_meas.orientation.z,pose_meas.orientation.w);
KDL::Rotation errorRotation = (m_traject_end.M)*(m_traject_begin.M.Inverse());
// KDL::Rotation tmp=errorRotation;
// double q1, q2, q3, q4;
// cout << "tmp" << endl;
// cout << tmp(0,0) << ", " << tmp(0,1) << ", " << tmp(0,2) << endl;
// cout << tmp(1,0) << ", " << tmp(1,1) << ", " << tmp(1,2) << endl;
// cout << tmp(2,0) << ", " << tmp(2,1) << ", " << tmp(2,2) << endl;
// cout << endl;
// tmp.GetQuaternion(q1,q2,q3,q4);
// cout << "tmp quaternion: " << q1 << q2 << q3 << q4 << endl;
double x,y,z,w;
errorRotation.GetQuaternion(x,y,z,w);
Eigen::AngleAxis<double> aa;
aa = Eigen::Quaterniond(w,x,y,z);
currentRotationalAxis = aa.axis();
deltaTheta = aa.angle();
// std::cout << "----EIGEN---------" << std::endl << "currentRotationalAxis: " << std::endl << currentRotationalAxis << std::endl;
// std::cout << "deltaTheta" << deltaTheta << std::endl;
// currentRotationalAxis[0]=x;
// currentRotationalAxis[1]=y;
// currentRotationalAxis[2]=z;
// if(currentRotationalAxis.norm() > 0.001){
// currentRotationalAxis.normalize();
// deltaTheta = 2*acos(w);
// }else{
// currentRotationalAxis.setZero();
// deltaTheta = 0.0;
// }
// std::cout << "----KDL-----------" << std::endl << "currentRotationalAxis: " << std::endl << currentRotationalAxis << std::endl;
// std::cout << "deltaTheta" << deltaTheta << std::endl;
std::vector<double> cartPositionCmd = std::vector<double>(3,0.0);
cartPositionCmd[0] = pose.position.x;
cartPositionCmd[1] = pose.position.y;
cartPositionCmd[2] = pose.position.z;
std::vector<double> cartPositionMsr = std::vector<double>(3,0.0);
//the following 3 assignments will get over written except for the first time
cartPositionMsr[0] = pose_meas.position.x;
cartPositionMsr[1] = pose_meas.position.y;
cartPositionMsr[2] = pose_meas.position.z;
std::vector<double> cartVelocity = std::vector<double>(3,0.0);
if ((int)motionProfile.size() == 0){//Only for the first run
for(int i = 0; i < 3; i++)
{
cartVelocity[i] = 0.0;
}
}else{
for(int i = 0; i < 3; i++)
{
cartVelocity[i] = motionProfile[i].Vel(m_time_passed);
cartPositionMsr[i] = motionProfile[i].Pos(m_time_passed);
}
}
motionProfile.clear();
// Set motion profiles
for(int i = 0; i < 3; i++){
motionProfile.push_back(VelocityProfile_NonZeroInit(m_maximum_velocity[i], m_maximum_acceleration[i]));
motionProfile[i].SetProfile(cartPositionMsr[i], cartPositionCmd[i], cartVelocity[i]);
}
//motionProfile for theta
motionProfile.push_back(VelocityProfile_NonZeroInit(m_maximum_velocity[3], m_maximum_acceleration[3]));
motionProfile[3].SetProfile(0.0,deltaTheta,0.0);
m_time_begin = os::TimeService::Instance()->getTicks();
m_time_passed = 0;
return true;
}
int main (int argc, char** argv)
{
std::string output;
std::vector<std::string> inputs, filenames; // filenames is for conf file
std::string conf_file_option, robothardware_conf_file_option, integrate("true"), dt("0.005"), timeStep(dt), joint_properties, method("EULER");
bool use_highgain_mode(true);
struct stat st;
bool file_exist_flag = false;
for (int i = 1; i < argc; ++ i) {
std::string arg(argv[i]);
normalize(arg);
if ( arg == "--output" ) {
if (++i < argc) output = argv[i];
} else if ( arg == "--integrate" ) {
if (++i < argc) integrate = argv[i];
} else if ( arg == "--dt" ) {
if (++i < argc) dt = argv[i];
} else if ( arg == "--timestep" ) {
if (++i < argc) timeStep = argv[i];
} else if ( arg == "--conf-file-option" ) {
if (++i < argc) conf_file_option += std::string("\n") + argv[i];
} else if ( arg == "--robothardware-conf-file-option" ) {
if (++i < argc) robothardware_conf_file_option += std::string("\n") + argv[i];
} else if ( arg == "--joint-properties" ) {
if (++i < argc) joint_properties = argv[i];
} else if ( arg == "--use-highgain-mode" ) {
if (++i < argc) use_highgain_mode = (std::string(argv[i])==std::string("true")?true:false);
} else if ( arg == "--method" ) {
if (++i < argc) method = std::string(argv[i]);
} else if ( arg.find("--gtest_output") == 0 ||arg.find("--text") == 0 || arg.find("__log") == 0 || arg.find("__name") == 0 ) { // skip
} else {
inputs.push_back(argv[i]);
}
}
CORBA::ORB_var orb = CORBA::ORB::_nil();
try
{
orb = CORBA::ORB_init(argc, argv);
CORBA::Object_var obj;
obj = orb->resolve_initial_references("RootPOA");
PortableServer::POA_var poa = PortableServer::POA::_narrow(obj);
if(CORBA::is_nil(poa))
{
throw string("error: failed to narrow root POA.");
}
PortableServer::POAManager_var poaManager = poa->the_POAManager();
if(CORBA::is_nil(poaManager))
{
throw string("error: failed to narrow root POA manager.");
}
xmlTextWriterPtr writer;
if (stat(output.c_str(), &st) == 0) {
file_exist_flag = true;
}
writer = xmlNewTextWriterFilename(output.c_str(), 0);
xmlTextWriterSetIndent(writer, 4);
xmlTextWriterStartElement(writer, BAD_CAST "grxui");
{
xmlTextWriterStartElement(writer, BAD_CAST "mode");
xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST "Simulation");
{
xmlTextWriterStartElement(writer, BAD_CAST "item");
xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.item.GrxSimulationItem");
xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST "simulationItem");
{
xmlTextWriterWriteProperty(writer, "integrate", integrate);
xmlTextWriterWriteProperty(writer, "timeStep", timeStep);
xmlTextWriterWriteProperty(writer, "totalTime", "2000000.0");
xmlTextWriterWriteProperty(writer, "method", method);
}
xmlTextWriterEndElement(writer); // item
// default WAIST offset
hrp::Vector3 WAIST_offset_pos = hrp::Vector3::Zero();
Eigen::AngleAxis<double> WAIST_offset_rot = Eigen::AngleAxis<double>(0, hrp::Vector3(0,0,1)); // rotation in VRML is represented by axis + angle
for (std::vector<std::string>::iterator it = inputs.begin();
it != inputs.end();
it++) {
// argument for VRML supports following two mode:
// 1. xxx.wrl
// To specify VRML file. WAIST offsets is 0 transformation.
// 2. xxx.wrl,0,0,0,0,0,1,0
// To specify both VRML and WAIST offsets.
// WAIST offset: for example, "0,0,0,0,0,1,0" -> position offset (3dof) + axis for rotation offset (3dof) + angle for rotation offset (1dof)
vstring filename_arg_str = split(*it, ",");
std::string filename = filename_arg_str[0];
filenames.push_back(filename);
if ( filename_arg_str.size () > 1 ){ // if WAIST offset is specified
for (size_t i = 0; i < 3; i++) {
stringTo(WAIST_offset_pos(i), filename_arg_str[i+1].c_str());
stringTo(WAIST_offset_rot.axis()(i), filename_arg_str[i+1+3].c_str());
}
stringTo(WAIST_offset_rot.angle(), filename_arg_str[1+3+3].c_str());
}
hrp::BodyPtr body(new hrp::Body());
BodyInfo_impl bI(poa);
bI.loadModelFile(filename.c_str());
ModelLoaderHelper2 helper;
helper.createBody(body, bI);
std::string name = body->name();
xmlTextWriterStartElement(writer, BAD_CAST "item");
xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.item.GrxRTSItem");
xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST name.c_str());
xmlTextWriterWriteAttribute(writer, BAD_CAST "select", BAD_CAST "true");
xmlTextWriterWriteProperty(writer, name+"(Robot)0.period", dt);
if (use_highgain_mode) {
xmlTextWriterWriteProperty(writer, "HGcontroller0.period", dt);
xmlTextWriterWriteProperty(writer, "HGcontroller0.factory", "HGcontroller");
if (it==inputs.begin()) {
xmlTextWriterWriteProperty(writer, "connection", "HGcontroller0.qOut:"+name+"(Robot)0.qRef");
xmlTextWriterWriteProperty(writer, "connection", "HGcontroller0.dqOut:"+name+"(Robot)0.dqRef");
xmlTextWriterWriteProperty(writer, "connection", "HGcontroller0.ddqOut:"+name+"(Robot)0.ddqRef");
}
} else {
xmlTextWriterWriteProperty(writer, "PDcontroller0.period", dt);
xmlTextWriterWriteProperty(writer, "PDcontroller0.factory", "PDcontroller");
if (it==inputs.begin()) {
xmlTextWriterWriteProperty(writer, "connection", "PDcontroller0.torque:"+name+"(Robot)0.tauRef");
xmlTextWriterWriteProperty(writer, "connection", ""+name+"(Robot)0.q:PDcontroller0.angle");
}
}
xmlTextWriterEndElement(writer); // item
xmlTextWriterStartElement(writer, BAD_CAST "item");
xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.item.GrxModelItem");
xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST (basename(*it).c_str()));
xmlTextWriterWriteAttribute(writer, BAD_CAST "url", BAD_CAST filename.c_str());
xmlTextWriterWriteProperty(writer, "rtcName", name + "(Robot)0");
if (use_highgain_mode) {
xmlTextWriterWriteProperty(writer, "inport", "qRef:JOINT_VALUE");
xmlTextWriterWriteProperty(writer, "inport", "dqRef:JOINT_VELOCITY");
xmlTextWriterWriteProperty(writer, "inport", "ddqRef:JOINT_ACCELERATION");
if (integrate == "false") { // For kinematics only simultion
xmlTextWriterWriteProperty(writer, "inport", "basePoseRef:"+body->rootLink()->name+":ABS_TRANSFORM");
}
} else {
xmlTextWriterWriteProperty(writer, "inport", "tauRef:JOINT_TORQUE");
}
xmlTextWriterWriteProperty(writer, "outport", "q:JOINT_VALUE");
xmlTextWriterWriteProperty(writer, "outport", "dq:JOINT_VELOCITY");
xmlTextWriterWriteProperty(writer, "outport", "tau:JOINT_TORQUE");
xmlTextWriterWriteProperty(writer, "outport", body->rootLink()->name+":"+body->rootLink()->name+":ABS_TRANSFORM");
// set outport for sensros
int nforce = body->numSensors(hrp::Sensor::FORCE);
if ( nforce > 0 ) std::cerr << "hrp::Sensor::FORCE";
for (unsigned int i=0; i<nforce; i++){
hrp::Sensor *s = body->sensor(hrp::Sensor::FORCE, i);
// port name and sensor name is same in case of ForceSensor
xmlTextWriterWriteProperty(writer, "outport", s->name + ":" + s->name + ":FORCE_SENSOR");
std::cerr << " " << s->name;
}
if ( nforce > 0 ) std::cerr << std::endl;
int ngyro = body->numSensors(hrp::Sensor::RATE_GYRO);
if ( ngyro > 0 ) std::cerr << "hrp::Sensor::GYRO";
if(ngyro == 1){
// port is named with no number when there is only one gyro
hrp::Sensor *s = body->sensor(hrp::Sensor::RATE_GYRO, 0);
xmlTextWriterWriteProperty(writer, "outport", s->name + ":" + s->name + ":RATE_GYRO_SENSOR");
std::cerr << " " << s->name;
}else{
for (unsigned int i=0; i<ngyro; i++){
hrp::Sensor *s = body->sensor(hrp::Sensor::RATE_GYRO, i);
std::stringstream str_strm;
str_strm << s->name << i << ":" + s->name << ":RATE_GYRO_SENSOR";
xmlTextWriterWriteProperty(writer, "outport", str_strm.str());
std::cerr << " " << s->name;
}
}
if ( ngyro > 0 ) std::cerr << std::endl;
int nacc = body->numSensors(hrp::Sensor::ACCELERATION);
if ( nacc > 0 ) std::cerr << "hrp::Sensor::ACCELERATION";
if(nacc == 1){
// port is named with no number when there is only one acc
hrp::Sensor *s = body->sensor(hrp::Sensor::ACCELERATION, 0);
xmlTextWriterWriteProperty(writer, "outport", s->name + ":" + s->name + ":ACCELERATION_SENSOR");
std::cerr << " " << s->name;
}else{
for (unsigned int i=0; i<nacc; i++){
hrp::Sensor *s = body->sensor(hrp::Sensor::ACCELERATION, i);
std::stringstream str_strm;
str_strm << s->name << i << ":" << s->name << ":ACCELERATION_SENSOR";
xmlTextWriterWriteProperty(writer, "outport", str_strm.str());
std::cerr << " " << s->name;
}
}
if ( nacc > 0 ) std::cerr << std::endl;
//
std::string root_name = body->rootLink()->name;
xmlTextWriterWriteProperty(writer, root_name+".NumOfAABB", "1");
// write waist pos and rot by considering both VRML original WAIST and WAIST_offset_pos and WAIST_offset_rot from arguments
std::ostringstream os;
os << body->rootLink()->p[0] + WAIST_offset_pos(0) << " "
<< body->rootLink()->p[1] + WAIST_offset_pos(1) << " "
<< body->rootLink()->p[2] + WAIST_offset_pos(2); // 10cm margin
xmlTextWriterWriteProperty(writer, root_name+".translation", os.str());
os.str(""); // reset ostringstream
Eigen::AngleAxis<double> tmpAA = Eigen::AngleAxis<double>(hrp::Matrix33(body->rootLink()->R * WAIST_offset_rot.toRotationMatrix()));
os << tmpAA.axis()(0) << " "
<< tmpAA.axis()(1) << " "
<< tmpAA.axis()(2) << " "
<< tmpAA.angle();
xmlTextWriterWriteProperty(writer, root_name+".rotation", os.str());
if ( ! body->isStaticModel() ) {
xmlTextWriterWriteProperty(writer, root_name+".mode", "Torque");
xmlTextWriterWriteProperty(writer, "controller", basename(output));
}
// store joint properties
// [property 1],[value 1],....
// ex. --joint-properties RARM_JOINT0.angle,0.0,RARM_JOINT2.mode,HighGain,...
vstring joint_properties_arg_str = split(joint_properties, ",");
std::map <std::string, std::string> joint_properties_map;
for (size_t i = 0; i < joint_properties_arg_str.size()/2; i++) {
joint_properties_map.insert(std::pair<std::string, std::string>(joint_properties_arg_str[i*2], joint_properties_arg_str[i*2+1]));
}
if ( body->numJoints() > 0 ) std::cerr << "hrp::Joint";
for(int i = 0; i < body->numJoints(); i++){
if ( body->joint(i)->index > 0 ) {
std::cerr << " " << body->joint(i)->name << "(" << body->joint(i)->jointId << ")";
std::string joint_name = body->joint(i)->name;
std::string j_property = joint_name+".angle";
xmlTextWriterWriteProperty(writer, j_property,
(joint_properties_map.find(j_property) != joint_properties_map.end()) ? joint_properties_map[j_property] : "0.0");
j_property = joint_name+".mode";
xmlTextWriterWriteProperty(writer, j_property,
(joint_properties_map.find(j_property) != joint_properties_map.end()) ? joint_properties_map[j_property] : (use_highgain_mode?"HighGain":"Torque"));
j_property = joint_name+".NumOfAABB";
xmlTextWriterWriteProperty(writer, j_property,
(joint_properties_map.find(j_property) != joint_properties_map.end()) ? joint_properties_map[j_property] : "1");
}
}
xmlTextWriterEndElement(writer); // item
if ( body->numJoints() > 0 ) std::cerr << std::endl;
//
// comment out self collision settings according to issues at https://github.com/fkanehiro/hrpsys-base/issues/122
// xmlTextWriterStartElement(writer, BAD_CAST "item");
// xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.item.GrxCollisionPairItem");
// xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST std::string("CP#"+name).c_str());
// xmlTextWriterWriteAttribute(writer, BAD_CAST "select", BAD_CAST "true");
// {
// xmlTextWriterWriteProperty(writer, "springConstant", "0 0 0 0 0 0");
// xmlTextWriterWriteProperty(writer, "slidingFriction", "0.5");
// xmlTextWriterWriteProperty(writer, "jointName2", "");
// xmlTextWriterWriteProperty(writer, "jointName1", "");
// xmlTextWriterWriteProperty(writer, "damperConstant", "0 0 0 0 0 0");
// xmlTextWriterWriteProperty(writer, "objectName2", name);
// xmlTextWriterWriteProperty(writer, "objectName1", name);
// xmlTextWriterWriteProperty(writer, "springDamperModel", "false");
// xmlTextWriterWriteProperty(writer, "staticFriction", "0.5");
// }
// xmlTextWriterEndElement(writer); // item
xmlTextWriterStartElement(writer, BAD_CAST "view");
xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.view.GrxRobotHardwareClientView");
xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST "RobotHardware RTC Client");
xmlTextWriterWriteProperty(writer, "robotHost", "localhost");
xmlTextWriterWriteProperty(writer, "StateHolderRTC", "StateHolder0");
xmlTextWriterWriteProperty(writer, "interval", "100");
xmlTextWriterWriteProperty(writer, "RobotHardwareServiceRTC", "RobotHardware0");
xmlTextWriterWriteProperty(writer, "robotPort", "2809");
xmlTextWriterWriteProperty(writer, "ROBOT", name.c_str());
xmlTextWriterEndElement(writer); // item
xmlTextWriterStartElement(writer, BAD_CAST "view");
xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.view.Grx3DView");
xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST "3DView");
xmlTextWriterWriteProperty(writer, "view.mode", "Room");
xmlTextWriterWriteProperty(writer, "showCoM", "false");
xmlTextWriterWriteProperty(writer, "showCoMonFloor", "false");
xmlTextWriterWriteProperty(writer, "showDistance", "false");
xmlTextWriterWriteProperty(writer, "showIntersection", "false");
xmlTextWriterWriteProperty(writer, "eyeHomePosition", "-0.70711 -0 0.70711 2 0.70711 -0 0.70711 2 0 1 0 0.8 0 0 0 1 ");
xmlTextWriterWriteProperty(writer, "showCollision", "true");
xmlTextWriterWriteProperty(writer, "showActualState", "true");
xmlTextWriterWriteProperty(writer, "showScale", "true");
xmlTextWriterEndElement(writer); // view
}
{
for (std::vector<std::string>::iterator it1 = inputs.begin(); it1 != inputs.end(); it1++) {
std::string name1 = basename(*it1);
for (std::vector<std::string>::iterator it2 = it1+1; it2 != inputs.end(); it2++) {
std::string name2 = basename(*it2);
xmlTextWriterStartElement(writer, BAD_CAST "item");
xmlTextWriterWriteAttribute(writer, BAD_CAST "class", BAD_CAST "com.generalrobotix.ui.item.GrxCollisionPairItem");
xmlTextWriterWriteAttribute(writer, BAD_CAST "name", BAD_CAST std::string("CP#"+name2+"_#"+name1+"_").c_str());
{
xmlTextWriterWriteProperty(writer, "springConstant", "0 0 0 0 0 0");
xmlTextWriterWriteProperty(writer, "slidingFriction", "0.5");
xmlTextWriterWriteProperty(writer, "jointName2", "");
xmlTextWriterWriteProperty(writer, "jointName1", "");
xmlTextWriterWriteProperty(writer, "damperConstant", "0 0 0 0 0 0");
xmlTextWriterWriteProperty(writer, "objectName2", name2);
xmlTextWriterWriteProperty(writer, "objectName1", name1);
xmlTextWriterWriteProperty(writer, "springDamperModel", "false");
xmlTextWriterWriteProperty(writer, "staticFriction", "0.5");
}
xmlTextWriterEndElement(writer); // item
}
}
}
}
xmlTextWriterEndElement(writer); // mode
}
xmlTextWriterEndElement(writer); // grxui
xmlFreeTextWriter(writer);
if (file_exist_flag) {
std::cerr << "\033[1;31mOver\033[0m";
}
std::cerr << "Writing project files to .... " << output << std::endl;
{
std::string conf_file = output.substr(0,output.find_last_of('.'))+".conf";
if (stat(conf_file.c_str(), &st) == 0) {
std::cerr << "\033[1;31mOver\033[0m";
}
std::fstream s(conf_file.c_str(), std::ios::out);
s << "model: file://" << filenames[0] << std::endl;
s << "dt: " << dt << std::endl;
s << conf_file_option << std::endl;
std::cerr << "Writing conf files to ....... " << conf_file << std::endl;
}
{
std::string conf_file = output.substr(0,output.find_last_of('.'))+".RobotHardware.conf";
if (stat(conf_file.c_str(), &st) == 0) {
std::cerr << "\033[1;31mOver\033[0m";
}
std::fstream s(conf_file.c_str(), std::ios::out);
s << "model: file://" << filenames[0] << std::endl;
s << "exec_cxt.periodic.type: hrpExecutionContext" << std::endl;
s << "exec_cxt.periodic.rate: " << static_cast<size_t>(1/atof(dt.c_str())+0.5) << std::endl; // rounding to specify integer rate value
s << robothardware_conf_file_option << std::endl;
std::cerr << "Writing hardware files to ... " << conf_file << std::endl;
}
}
catch (CORBA::SystemException& ex)
{
cerr << ex._rep_id() << endl;
}
catch (const string& error)
{
cerr << error << endl;
}
try
{
orb->destroy();
}
catch(...)
{
}
return 0;
}
static Vector3s matrixToAngleAxis( const Matrix33sr& R )
{
const Eigen::AngleAxis<scalar> R_aa{ R };
return R_aa.angle() * R_aa.axis();
}
bool CartesianGenerator::generateNewVelocityProfiles(RTT::base::PortInterface* portInterface)
{
m_time_passed = os::TimeService::Instance()->secondsSince(m_time_begin);
geometry_msgs::Pose pose;
cmdCartPose.read(pose);
desired_pose = pose;
#if 1
std::cout << "A new pose arrived" << std::endl;
std::cout << "position: " << pose.position.x << " " << pose.position.y << " " << pose.position.z << std::endl;
#endif
m_traject_end.p.x(pose.position.x);
m_traject_end.p.y(pose.position.y);
m_traject_end.p.z(pose.position.z);
m_traject_end.M = Rotation::Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w);
// get current position
geometry_msgs::Pose pose_meas;
m_position_meas_port.read(pose_meas);
m_traject_begin.p.x(pose_meas.position.x);
m_traject_begin.p.y(pose_meas.position.y);
m_traject_begin.p.z(pose_meas.position.z);
m_traject_begin.M = Rotation::Quaternion(pose_meas.orientation.x, pose_meas.orientation.y, pose_meas.orientation.z,
pose_meas.orientation.w);
xi = pose_meas.position.x;
yi = pose_meas.position.y;
zi = pose_meas.position.z;
xf = pose.position.x;
yf = pose.position.y;
zf = pose.position.z;
TrajVectorMagnitude = sqrt((xf - xi) * (xf - xi) + (yf - yi) * (yf - yi) + (zf - zi) * (zf - zi));
TrajVectorDirection.x = (xf - xi) / TrajVectorMagnitude;
TrajVectorDirection.y = (yf - yi) / TrajVectorMagnitude;
TrajVectorDirection.z = (zf - zi) / TrajVectorMagnitude;
double tx, ty, tz;
tx = abs(xf - xi) / m_maximum_velocity[0];
ty = abs(yf - yi) / m_maximum_velocity[0];
tz = abs(zf - zi) / m_maximum_velocity[0];
t_sync = TrajVectorMagnitude / m_maximum_velocity[0];
KDL::Rotation errorRotation = (m_traject_end.M) * (m_traject_begin.M.Inverse());
double x, y, z, w;
errorRotation.GetQuaternion(x, y, z, w);
Eigen::AngleAxis<double> aa;
aa = Eigen::Quaterniond(w, x, y, z);
currentRotationalAxis = aa.axis();
deltaTheta = aa.angle();
theta_vel = deltaTheta / t_sync;
cout << "[CG::GenerateProfiles]:" << endl;
m_time_begin = os::TimeService::Instance()->getTicks();
m_time_passed = 0;
return true;
}