int ComauSmartSix::ik(float x, float y,float z,float qx, float qy,float qz, float qw, float* q_in,float* q_out) {
KDL::Rotation rot = KDL::Rotation::Quaternion(qx,qy,qz,qw);
double roll,pitch,yaw;
rot.GetRPY(roll,pitch,yaw);
return this->ik(x,y,z,roll,pitch,yaw,q_in,q_out,true);
}
void DetectorFilter::decomposeTransform(const geometry_msgs::PoseWithCovarianceStamped& pose,
MatrixWrapper::ColumnVector& vector)
{
assert(vector.rows() == 6);
// construct kdl rotation from quaternion, and extract RPY
KDL::Rotation rot = KDL::Rotation::Quaternion(pose.pose.pose.orientation.x,
pose.pose.pose.orientation.y,
pose.pose.pose.orientation.z,
pose.pose.pose.orientation.w);
rot.GetRPY(vector(4), vector(5), vector(6));
vector(1) = pose.pose.pose.position.x;
vector(2) = pose.pose.pose.position.y;
vector(3) = pose.pose.pose.position.z;
};
void YouBotBaseService::update()
{
if(is_in_visualization_mode)
{
in_odometry_state.read(m_odometry_state);
simxFloat pos[3];
pos[0] = m_odometry_state.pose.pose.position.x;
pos[1] = m_odometry_state.pose.pose.position.y;
pos[2] = m_odometry_state.pose.pose.position.z;
simxSetObjectPosition(clientID,all_robot_handle,-1,pos,simx_opmode_oneshot);
double euler[3];
simxFloat euler_s[3];
//different coordinate systems, fixing it here
KDL::Rotation orientation = KDL::Rotation::Quaternion(
m_odometry_state.pose.pose.orientation.x,
m_odometry_state.pose.pose.orientation.y,
m_odometry_state.pose.pose.orientation.z,
m_odometry_state.pose.pose.orientation.w);
// Instead of transforming for the inverse of this rotation,
// we should find the right transform. (this is legacy code that
// transforms vrep coords to rviz coords)
KDL::Rotation rotation = KDL::Rotation::RPY(0,-M_PI/2,M_PI).Inverse();
orientation = orientation * rotation;
orientation.GetRPY(euler[0],euler[1],euler[2]);
euler_s[0] = euler[0];
euler_s[1] = euler[1];
euler_s[2] = euler[2];
simxSetObjectOrientation(clientID,all_robot_handle,-1,euler_s,simx_opmode_oneshot);
simxSetJointPosition(clientID,vrep_joint_handle[0],m_joint_state.position[0], simx_opmode_oneshot);
simxSetJointPosition(clientID,vrep_joint_handle[1],m_joint_state.position[1], simx_opmode_oneshot);
simxSetJointPosition(clientID,vrep_joint_handle[2],m_joint_state.position[2], simx_opmode_oneshot);
simxSetJointPosition(clientID,vrep_joint_handle[3],m_joint_state.position[3], simx_opmode_oneshot);
return;
}
Hilas::IRobotBaseService::update();
}
