tf::Stamped<tf::Pose> getHandlePose(const door_msgs::Door& door, int side) { Vector x_axis(1,0,0); double dist = sqrt(pow(door.frame_p1.x - door.handle.x,2)+pow(door.frame_p1.y - door.handle.y,2)); if(door.hinge == door_msgs::Door::HINGE_P2) { dist = sqrt(pow(door.frame_p2.x - door.handle.x,2)+pow(door.frame_p2.y - door.handle.y,2)); } double angle = getDoorAngle(door); // get hinge point Vector hinge, frame_vec; if (door.hinge == door_msgs::Door::HINGE_P1){ hinge = Vector(door.door_p1.x, door.door_p1.y, door.door_p1.z); frame_vec = Vector(door.frame_p2.x - door.frame_p1.x, door.frame_p2.y - door.frame_p1.y, door.frame_p2.z - door.frame_p1.z); } else if (door.hinge == door_msgs::Door::HINGE_P2){ hinge = Vector(door.door_p2.x, door.door_p2.y, door.door_p2.z); frame_vec = Vector(door.frame_p1.x - door.frame_p2.x, door.frame_p1.y - door.frame_p2.y, door.frame_p1.z - door.frame_p2.z); } // get gripper pos frame_vec.Normalize(); frame_vec = frame_vec * dist; Rotation rot_angle = Rotation::RotZ(angle); Vector handle_pos = hinge + (rot_angle * frame_vec); tf::Stamped<tf::Pose> handle_pose; Vector normal_frame = getFrameNormal(door); if(side == -1) { normal_frame = -normal_frame; } handle_pose.frame_id_ = door.header.frame_id; handle_pose.stamp_ = door.header.stamp; handle_pose.setOrigin( tf::Vector3(handle_pos(0), handle_pos(1), gripper_height)); handle_pose.setRotation( tf::createQuaternionFromRPY(getVectorAngle(x_axis, normal_frame)+angle, 0, 0) ); tf::Pose gripper_rotate(tf::createQuaternionFromRPY(0.0,0.0,M_PI/2.0),tf::Vector3(0.0,0.0,0.0)); handle_pose.mult(handle_pose,gripper_rotate); return handle_pose; }
int julia_key_events(int key, t_env *e) { if (key == ESC) exit(0); if (key == KEY_C) handle_color(e); if (key == UP || key == DOWN || key == LEFT || key == RIGHT) handle_pos(key, e->j); if (key == KEY_R) init_julia(e->j, e); if (key == KEY_H) e->help = !e->help ? 1 : 0; expose_hook(e); return (1); }
int ft_key_hook(int keycode, t_env *env) { if (keycode == ESC) exit_program(keycode, 0); else if (keycode == MINUS || keycode == PLUS) handle_size(keycode, env); else if (keycode == LEFT || keycode == RIGHT || keycode == UP || keycode == DOWN) handle_pos(keycode, env); else if (keycode == SLASH || keycode == STAR) handle_spike(keycode, env); else if (keycode == KEY_C) { env->ui = !env->ui ? 1 : 0; expose_hook(env); } return (0); }