// MASTER MEASUREMENTS
void MasterController::masterJointsCallback(const sensor_msgs::JointState::ConstPtr& joint_states)
{
double x_master=(master_max(0,0)-master_min(0,0))/2.0+master_min(0,0);
double y_master=(master_max(1,0)-master_min(1,0))/2.0+master_min(1,0);
double z_master=(master_max(2,0)-master_min(2,0))/2.0+master_min(2,0);
double yaw_master_joint=joint_states->position[5];
double yaw_master=0.0;
if(linear_button_pressed)
{
try
{
listener.lookupTransform("/base", "/wrist2",ros::Time(0), transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
}
x_master=transform.getOrigin().x();
if(x_master<master_min(0,0))
{
x_master=master_min(0,0);
}
else if(x_master>master_max(0,0))
{
x_master=master_max(0,0);
}
y_master=transform.getOrigin().y();
if(y_master<master_min(1,0))
{
y_master=master_min(1,0);
}
else if(y_master>master_max(1,0))
{
y_master=master_max(1,0);
}
z_master=transform.getOrigin().z();
if(z_master<master_min(2,0))
{
z_master=master_min(2,0);
}
else if(z_master>master_max(2,0))
{
z_master=master_max(2,0);
}
}
if(angular_button_pressed)
{
// Wrist3 controls angular speed
if(yaw_master_joint<master_min(5,0))
{
yaw_master_joint=master_min(5,0);
}
else if(yaw_master_joint>master_max(5,0))
{
yaw_master_joint=master_max(5,0);
}
yaw_master=yaw_master_joint-yaw_master_joint_previous; // delta q - desired position
}
yaw_master_joint_previous=yaw_master_joint;
yaw_master_joint_previous=yaw_master_joint;
ros::Time current_time=ros::Time::now();
double period = current_time.toSec()-previous_time.toSec();
previous_time=current_time;
//std::cout << "period:"<< period << std::endl;
// Pose master
// x and y are mirrored
// angles are relative
current_pose_master <<
(-x_master + master_min(0,0)+master_max(0,0)),
(-y_master + master_min(1,0)+master_max(1,0)),
z_master,
0.0,
0.0,
yaw_master;
haptic_position.header.stamp = ros::Time::now();
haptic_pub.publish(haptic_position);
current_velocity_master=(current_pose_master-previous_pose_master)/period;
haptic_position.pose.pose.position.x = -x_master + master_min(0,0)+master_max(0,0);
haptic_position.pose.pose.position.y = -y_master + master_min(1,0)+master_max(1,0);
haptic_position.pose.pose.position.z = z_master ;
// haptic_position.twist.twist.linear.x = current_velocity_master(0,0) ;
// haptic_position.twist.twist.linear.x = current_velocity_master(0,1) ;
// haptic_position.twist.twist.linear.x = current_velocity_master(0,2) ;
master_new_readings=true;
//feedback();
previous_pose_master=current_pose_master;
}
// MASTER MEASUREMENTS
void SlaveController::masterJointsCallback(const sensor_msgs::JointState::ConstPtr& joint_states)
{
double x_master=(master_max(0,0)-master_min(0,0))/2.0+master_min(0,0);
double y_master=(master_max(1,0)-master_min(1,0))/2.0+master_min(1,0);
double z_master=(master_max(2,0)-master_min(2,0))/2.0+master_min(2,0);
double yaw_master_joint=joint_states->position[5];
double yaw_master=0.0;
if(linear_button_pressed)
{
try
{
listener.lookupTransform("/base", "/wrist2",ros::Time(0), transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
}
x_master=transform.getOrigin().x();
if(x_master<master_min(0,0))
{
x_master=master_min(0,0);
}
else if(x_master>master_max(0,0))
{
x_master=master_max(0,0);
}
y_master=transform.getOrigin().y();
if(y_master<master_min(1,0))
{
y_master=master_min(1,0);
}
else if(y_master>master_max(1,0))
{
y_master=master_max(1,0);
}
z_master=transform.getOrigin().z();
if(z_master<master_min(2,0))
{
z_master=master_min(2,0);
}
else if(z_master>master_max(2,0))
{
z_master=master_max(2,0);
}
}
if(angular_button_pressed)
{
// Wrist3 controls angular speed
if(yaw_master_joint<master_min(5,0))
{
yaw_master_joint=master_min(5,0);
}
else if(yaw_master_joint>master_max(5,0))
{
yaw_master_joint=master_max(5,0);
}
yaw_master=yaw_master_joint-yaw_master_joint_previous; // delta q - desired position
}
yaw_master_joint_previous=yaw_master_joint;
ros::Time current_time=ros::Time::now();
double period = current_time.toSec()-previous_time.toSec();
previous_time=current_time;
//std::cout << "period:"<< period << std::endl;
// Pose master
// x and y are mirrored
// angles are relative
current_pose_master <<
(-x_master + master_min(0,0)+master_max(0,0)),
(-y_master + master_min(1,0)+master_max(1,0)),
z_master,
0.0,
0.0,
yaw_master;
current_velocity_master=(current_pose_master-previous_pose_master)/period;
//double yaw_master_scaled=yaw_master*master_to_slave_scale(5,0);
/////////////////////////
// Scale to slave side //
/////////////////////////
// x_m, y_m, z_m maps to velocities in slave side
current_pose_master_scaled(0,0)=(current_pose_master(0,0)-master_min(0,0))*master_pose_slave_velocity_scale(0,0)+slave_velocity_min(0,0);
current_pose_master_scaled(1,0)=(current_pose_master(1,0)-master_min(1,0))*master_pose_slave_velocity_scale(1,0)+slave_velocity_min(1,0);
current_pose_master_scaled(2,0)=(current_pose_master(2,0)-master_min(2,0))*master_pose_slave_velocity_scale(2,0)+slave_velocity_min(2,0);
// relative angular position changes in master side maps to relative angular position changes in slave side
current_pose_master_scaled(3,0)=(current_pose_master(3,0))*master_to_slave_scale(3,0);
current_pose_master_scaled(4,0)=(current_pose_master(4,0))*master_to_slave_scale(4,0);
current_pose_master_scaled(5,0)=(current_pose_master(5,0))*master_to_slave_scale(5,0);
// Velocity master
current_velocity_master_scaled=(current_pose_master_scaled-previous_pose_master_scaled)/period;
master_new_readings=true;
feedback();
previous_pose_master=current_pose_master;
previous_pose_master_scaled=current_pose_master_scaled;
}
