def callBack(msg):
pose_vel = PoseVelocity()
e = tft.euler_from_quaternion([
msg.glovesData[0].wristIMU.x, msg.glovesData[0].wristIMU.y,
msg.glovesData[0].wristIMU.z, msg.glovesData[0].wristIMU.w
])
print(e)
x = e[0]
y = e[1]
z = e[2]
# 旋转 just for left hand, horizon is zero；
if y > 1.2 or y < -0.5:
if y > 1.2: #right
pose_vel.twist_angular_z = -0.1
elif y < -0.5: #left
pose_vel.twist_angular_z = 0.1
elif z > 1 or z < -1:
if z > 1: # up
pose_vel.twist_angular_y = -0.1
elif z < -1:
pose_vel.twist_angular_y = 0.1
elif x > 1 or x < -1:
if x > 1: # ni
pose_vel.twist_angular_z = -0.1
elif x < -1: # shun
pose_vel.twist_angular_z = 0.1
# 平移
if msg.glovesData[0].fingersFlex[1].Joint1Stretch > 0.3:
pose_vel.twist_linear_z = -0.1
if msg.glovesData[0].fingersFlex[1].Joint1Stretch < -0.3:
pose_vel.twist_linear_z = 0.1
if msg.glovesData[0].fingersFlex[2].Joint1Stretch > 0.3:
pose_vel.twist_linear_y = -0.1
if msg.glovesData[0].fingersFlex[2].Joint1Stretch < -0.3:
pose_vel.twist_linear_y = 0.1
if msg.glovesData[0].fingersFlex[3].Joint1Stretch > 0.3:
pose_vel.twist_linear_x = -0.1
if msg.glovesData[0].fingersFlex[3].Joint1Stretch < -0.3:
pose_vel.twist_linear_x = 0.1
# CURRENT_VELOCITY[3] = 1 * dr #x: end effector self rotate,positive is shun
# CURRENT_VELOCITY[4] = 1 * dp #y: up and down rotate (positive is down)
# CURRENT_VELOCITY[5] = 1 * dyaw #z: left and right rotate,positive is left
pub.publish(pose_vel)
def run(self):
rate = rospy.Rate(100)
while not rospy.is_shutdown():
vel = PoseVelocity()
vel.twist_linear_x = self.x
vel.twist_linear_y = self.y
vel.twist_linear_z = self.z
vel.twist_angular_x = 0.0
vel.twist_angular_y = 0.0
vel.twist_angular_z = 0.0
self.pub.publish(vel)
rate.sleep()
def publishArmLinControl(self):
pose_vel_msg = PoseVelocity()
pose_vel_msg.twist_linear_x = self.joy.axes[
1] * self.v_lin_gain # Up/Down Axis stick left
pose_vel_msg.twist_linear_y = self.joy.axes[
0] * self.v_lin_gain # Left/Right Axis stick left
pose_vel_msg.twist_linear_z = self.joy.axes[
3] * self.v_lin_gain # Up/Down Axis stick right
pose_vel_msg.twist_angular_x = 0
pose_vel_msg.twist_angular_y = 0
pose_vel_msg.twist_angular_z = 0
self.pub_pose_vel_cmd.publish(pose_vel_msg)
def publishArmAngControl(self):
pose_vel_msg = PoseVelocity()
pose_vel_msg.twist_linear_x = 0
pose_vel_msg.twist_linear_y = 0
pose_vel_msg.twist_linear_z = 0
pose_vel_msg.twist_angular_x = self.joy.axes[
1] * self.w_ang_gain # Up/Down Axis stick left
if self.joy.buttons[1] == 0: # if lip is not pressed
pose_vel_msg.twist_angular_y = self.joy.axes[
0] * self.w_ang_gain # Left/Right Axis stick left
elif self.joy.buttons[1] == 1: # if lip is pressed
pose_vel_msg.twist_angular_z = self.joy.axes[
0] * -self.w_ang_gain # Left/Right Axis stick right
self.pub_pose_vel_cmd.publish(pose_vel_msg)
def publishArmLinControl(self):
pose_vel_msg = PoseVelocity()
pose_vel_msg.twist_linear_x = self.joy.axes[
1] * self.v_lin_gain # Left\Right
if self.joy.buttons[1] == 0: # if lip is not pressed
pose_vel_msg.twist_linear_y = self.joy.axes[
0] * self.v_lin_gain # Forward\Backward
elif self.joy.buttons[1] == 1: # if lip is pressed
pose_vel_msg.twist_linear_z = self.joy.axes[
0] * self.v_lin_gain # Up/Down Axis stick right
pose_vel_msg.twist_angular_x = 0
pose_vel_msg.twist_angular_y = 0
pose_vel_msg.twist_angular_z = 0
self.pub_pose_vel_cmd.publish(pose_vel_msg)
def goup():
global vel, CURR_POSE, point_reached
desired_z_up = 0.3
current_pose = CURR_POSE
position_error = desired_z_up - current_pose.position.z
K_p = 4
thresh = 0.005
if abs(position_error) > thresh:
vel_z = K_p * position_error
else:
point_reached = True
vel_z = 0.0
print("point reached: go up")
global robot_type
if robot_type == 'kinova':
from kinova_msgs.msg import PoseVelocity
vel = PoseVelocity()
vel.twist_linear_x = 0.0
vel.twist_linear_y = 0.0
vel.twist_linear_z = vel_z
def servo_to_point(desired_centroid, depth, enable_z):
global vel, point_reached, CURR_FORCE, CURR_POSE, count_against_stuck
max_count_against_stuck = 300
if centroid:
cx_b = centroid[0]; cy_b = centroid[1];
current_centroid = np.array([cx_b, cy_b])
vel_z = 0.0
thresh = 10
max_norm = 0.05
e = current_centroid - desired_centroid
error_norm =np.linalg.norm(e)
if error_norm > thresh and count_against_stuck < max_count_against_stuck:
interaction = get_interaction(current_centroid, im_size, depth)
i_inv = np.linalg.pinv(interaction)
gain_vs = 0.008
velocity_cam = np.matmul(i_inv, e)
# velocity_b = cam_to_base(velocity_cam)[:2]
velocity_b = velocity_cam
velocity_b[1] = -velocity_b[1]
velocity_b = gain_vs*velocity_b
norm = np.linalg.norm(velocity_b)
if norm > max_norm:
velocity_b = (velocity_b / norm) * max_norm
if enable_z:
# Contact Management
#desired_force = -14.0
desired_force = -5.0
force_error = desired_force - CURR_FORCE
#K_d = 0.008
K_d = 0.0 #0.001
current_pose = CURR_POSE
if tool == "straight":
desired_z = 0.19
else:
desired_z = 0.15
position_error = desired_z - current_pose.position.z
#print("current z position: ", current_pose.position.z)
K_p = 5
#vel_z = K_d * force_error
vel_z = K_d * force_error + K_p * position_error
#vel_z = K_p * position_error
max_vel_z = 0.03
#vel_z = np.clip(vel_z, -max_vel_z, max_vel_z)
#print(vel_z)
count_against_stuck += 1
print("count: ", count_against_stuck)
else:
point_reached = True
#print("Point Reached?", point_reached)
velocity_b = np.array([0, 0])
count_against_stuck = 0
else:
print("No Assigned Centroid")
velocity_b = np.array([0, 0])
global robot_type
if robot_type == 'kinova':
from kinova_msgs.msg import PoseVelocity
vel = PoseVelocity()
vel.twist_linear_x = velocity_b[0]
vel.twist_linear_y = velocity_b[1]
vel.twist_linear_z = vel_z
#TODO: UR5 implementation
print("vel: ", velocity_b)