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);
}
