/*
* void Matrix4f::Rotation(float f_angle, const Vector3f &r_v_axis)
* - creates matrix for rotation arround axis given by r_v_axis,
* f_angle is angle in radians
*/
void Matrix4f::Rotation(float f_angle, const Vector3f &r_v_axis)
{
// formula for rotation matrix around arbitrary axis:
// R = uuT + cos(f_angle) * (I - uuT) + sin(f_angle)S
float f_cos = float(cos(f_angle));
Vector3f v_u_o_m_cos(r_v_axis * (1 - f_cos));
for(int i = 0; i < 3; ++ i) {
for(int j = 0; j < 3; ++ j)
f[i][j] = r_v_axis[i] * v_u_o_m_cos[j] + ((i == j)? f_cos : 0);
f[i][3] = 0;
f[3][i] = 0;
}
f[3][3] = 1;
// R = uu^T * (1 - cos(f_angle)) + cos(f_angle) * I + ...
Vector3f v_s(r_v_axis * float(sin(f_angle)));
f[1][0] -= v_s.z;
f[0][1] += v_s.z;
f[2][0] += v_s.y;
f[0][2] -= v_s.y;
f[2][1] -= v_s.x;
f[1][2] += v_s.x;
// ... + sin(f_angle)S
}
void LWRController::pubStatus()
{
ros::Publisher pub_js = mNode.advertise<sensor_msgs::JointState>("/joint_states", 1);
ros::Publisher pub_force_vis = mNode.advertise<visualization_msgs::Marker>("force_visual", 1);
ros::Publisher pub_force = mNode.advertise<geometry_msgs::PoseStamped>("force", 1);
ros::Publisher pub_ptool = mNode.advertise<geometry_msgs::PoseStamped>("pose_tool", 1);
std::string joint_names;
mNode.getParam("joint_names", joint_names);
while (is_running) {
boost::unique_lock<boost::mutex> lock(mFriClient.mMutex);
mFriClient.mCondSend.timed_wait(lock, boost::posix_time::milliseconds(150));
// TODO: Should check for condition=true; measure runtime here!
if (mFriClient.mSendStatus) {
mFriClient.mSendStatus = false;
// ==== JOINT STATES
sensor_msgs::JointState js;
js.header.stamp = ros::Time::now();
js.name.resize(7);
js.position.resize(7);
for (uint i=0; i<7; i++) {
char name[256];
sprintf(name, joint_names.c_str(), i);
js.name[i] = std::string(name);
js.position[i] = mFriClient.mMeasuredJoint[i];
}
pub_js.publish(js);
// ==== Cartesian Pose
float *pval = mFriClient.mMeasuredCart;
geometry_msgs::PoseStamped p_cart;
mFriClient.pose_kuka2ros(pval, p_cart.pose, NULL, 1);
p_cart.header.frame_id = "/calib_lwr_arm_base_link";
pub_ptool.publish(p_cart);
// ==== ESTIMATED FORCES
// Looks like this pose is in the Base frame, and the z-axis is negated compared to ROS
if (mFriClient.mForcesFrame > 0) {
geometry_msgs::PoseStamped pF_base, pF_hand;
pF_base.pose.position.x = mFriClient.mForces[0];
pF_base.pose.position.y = mFriClient.mForces[1];
pF_base.pose.position.z = mFriClient.mForces[2];
// TODO: Quaternion!
pF_base.pose.orientation.w = 1;
pF_base.pose.orientation.x = 0;
pF_base.pose.orientation.y = 0;
pF_base.pose.orientation.z = 0;
pF_base.header.stamp = ros::Time();
if (mFriClient.mForcesFrame == 1) {
//pF_base.pose.position.z *= -1.0;
pF_base.header.frame_id = "/calib_lwr_arm_base_link";
}
if (mFriClient.mForcesFrame == 2) {
pF_base.header.frame_id = "/lwr_arm_7_link";
}
try {
mTF.transformPose("/lwr_arm_7_link", pF_base, pF_hand);
pub_force.publish(pF_hand);
Eigen::Vector3d vF(pF_hand.pose.position.x, pF_hand.pose.position.y, pF_hand.pose.position.z);
Eigen::Vector3d v_s(0,0,0.1);
Eigen::Vector3d v_e = v_s + 0.1*vF;
visualization_msgs::Marker marker;
marker.header.frame_id = "/lwr_arm_7_link";
marker.header.stamp = ros::Time();
marker.ns = "my_namespace";
marker.id = 0;
marker.type = visualization_msgs::Marker::ARROW;
marker.action = visualization_msgs::Marker::ADD;
marker.points.resize(2);
marker.points[0].x = v_s[0];
marker.points[0].y = v_s[1];
marker.points[0].z = v_s[2];
marker.points[1].x = v_e[0];
marker.points[1].y = v_e[1];
marker.points[1].z = v_e[2];
marker.scale.x = 0.01;
marker.scale.y = 0.02;
marker.scale.z = 0.02;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 1.0;
pub_force_vis.publish(marker);
} catch (...) {
ROS_ERROR_STREAM("pubStatus: Failed to transform pose");
}
}
//
}
}
ROS_INFO_STREAM("LWRController::pubStatus: Exit");
}
void exportTerrain::printPolygonData(const MFnMesh &theMesh,const bool phy)
{
MStatus status;
MPointArray vertices;
MIntArray perPolyVertices, numVerticesPerPoly;
MVector tNorm;
MVectorArray perVertexNormals, normals;
MItMeshPolygon itPoly( theMesh.object() );
long numPolygons = theMesh.numPolygons();
long numVertices = theMesh.numVertices();
theMesh.getPoints(vertices);
fout << "Vertices: " << endl;
fout << "Number of Vertices: " << numVertices << endl;
fout << vertices << endl;
fout << "Number of polygons: " << numPolygons << endl;
fout << "Polygon Connection List:" << endl;
fout << "[";
for(int i = 0; i < numPolygons - 1; ++i){
fout << i << ": ";
status = theMesh.getPolygonVertices(i, perPolyVertices);
fout << perPolyVertices;
fout << ", " << endl;
}
//last is a special case
fout << numPolygons -1 << ": ";
status = theMesh.getPolygonVertices(numPolygons -1, perPolyVertices);
fout << perPolyVertices;
fout << "]" << endl;
//Per Vertex Normals
for(i=0; i < numVertices; ++i){
theMesh.getVertexNormal( i, tNorm);
perVertexNormals.append(tNorm);
}
fout << "Per Vertex Normals: \n";
fout << perVertexNormals << endl;
/* //per vertex per polygon Normals
//Not supported by the Reaper graphic engine
fout << "Normals" << endl;
i = 0;
fout << "[ ";
while (!itPoly.isDone() ){
itPoly.getNormals(normals);
fout << i << ": " << normals << endl;
++i;
itPoly.next();
}
fout << " ]" << endl;
*/
if( !phy ){
//Texture coordinate information
MIntArray ids;
int index;
MFloatArray Us, Vs;
theMesh.getUVs(Us,Vs);
MFloatArray u_s(numVertices),v_s(numVertices);
MIntArray numuvs(numVertices,0);
//Now the tactic is:
//Iterate over all polygons, and for each vertex read out the UV out of the list
//Insert that in two float arrays, and keep track of how many uvs
//that have been inserted. At last divd the us and vs by the number you
//already go there
//Shared uvs will look strange, but it is the average uvs that is shown
//Make sure to model using no shared uvs
for(i = 0;i<numPolygons;++i){
MIntArray polyVertices;
theMesh.getPolygonVertices(i,polyVertices);
for(int j = 0;j<polyVertices.length();++j){
int uv_id;
theMesh.getPolygonUVid(i,j,uv_id);
u_s[polyVertices[j]] += Us[uv_id];
v_s[polyVertices[j]] += Vs[uv_id];
numuvs[polyVertices[j]]+=1;
}
}
for(int k = 0;k<numVertices;++k){
u_s[k] /= numuvs[k];
v_s[k] /= numuvs[k];
}
fout << "Texture Coordinates: \n";
fout << "[" ;
for(index = 0;index < (u_s.length() - 1); ++index){
fout << index << ": [" << u_s[index] << ", " <<
v_s[index] << "]," << endl;
}
//last is a special case
fout << index << ": [" << u_s[index] << ", " <<
v_s[index] << "]";
fout << "]" << endl;
//Color data, for coloring. Make sure
MItMeshVertex vertexIt(theMesh.object());
MColor color;
std::vector<MColor> colorArray;
while (!vertexIt.isDone() ){
vertexIt.getColor(color);
colorArray.push_back(color);
vertexIt.next();
}
fout << "PerVertexColors: \n";
fout << "[" ;
for(index = 0;index < colorArray.size()-1; ++index){
fout << index << ": " << colorArray[index] << "," << endl;
}
//last is a special case
fout << index << ": " << colorArray[index];
fout << "]" << endl;
}
}
