bool receiveXYZRPY(kinematics::KinXYZRPY::Request &req, kinematics::KinXYZRPY::Response &res){ //ROS_INFO("xyz (%0.2f, %0.2f, %0.2f), RPY (%0.2f, %0.2f, %0.2f)", req.x, req.y, req.z, req.R, req.P, req.Y); if (req.from_current){ device->setQ(positions, state); } Transform3D<double> Tcurrent, Tdesired; Tcurrent = device->baseTend(state); Tdesired.P() = Vector3D<double>(req.x, req.y, req.z); Tdesired.R() = RPY<double>(req.R*Pi/180, req.P*Pi/180, req.Y*Pi/180).toRotation3D(); res.success = sendConfigurations(Tcurrent, Tdesired, state, req.interpolate, req.force_angle, req.angle); return true; }
void calculatePose(Q q){ State tmpState = wc->getDefaultState(); device->setQ(q, tmpState); Transform3D<double> cPose = device->baseTend(tmpState); RPY<double> rpy(cPose.R()); pose.x = cPose.P()[0]; pose.y = cPose.P()[1]; pose.z = cPose.P()[2]; pose.R = rpy(0)*180/Pi; pose.P = rpy(1)*180/Pi; pose.Y = rpy(2)*180/Pi; //cout << "POOOOOSE: " << cPose.P() << "\n"; kinematics::Pose msg; msg.x = cPose.P()[0]; msg.y = cPose.P()[1]; msg.z = cPose.P()[2]; msg.R = rpy(0)*180/Pi; msg.P = rpy(1)*180/Pi; msg.Y = rpy(2)*180/Pi; pose_pub.publish(msg); }
bool receiveJog(kinematics::KinXYZRPY::Request &req, kinematics::KinXYZRPY::Response &res){ device->setQ(positions, state); cout << "POSITIONS: " << positions << "\n"; Transform3D<double> Tcurrent = device->baseTend(state); Transform3D<double> Tdesired = device->baseTend(state); Tdesired.P() += Vector3D<double>(req.x, req.y, req.z); //Rotation3D<double> rot = RPY<double>(req.R, req.P, req.Y).toRotation3D(); /* Tdesired.R() = Rotation3D<double>( Tcurrent.R(0,0)+rot(0,0), Tcurrent.R(0,1)+rot(0,1), Tcurrent.R(0,2)+rot(0,2), Tcurrent.R(1,0)+rot(1,0), Tcurrent.R(1,1)+rot(1,1), Tcurrent.R(1,2)+rot(1,2), Tcurrent.R(2,0)+rot(2,0), Tcurrent.R(2,1)+rot(2,1), Tcurrent.R(2,1)+rot(2,2), ); */ res.success = sendConfigurations(Tcurrent, Tdesired, state, req.interpolate); return true; }