def commander(self, distance, speed):
vel = PoseVelocity()
beginTime = rospy.Time.now()
endTime = rospy.Duration(distance / float(abs(speed))) + beginTime
while rospy.Time.now() < beginTime:
vel.twist_linear_x = speed
self.pub.publish(vel)
def __init__(self): """Uses the kinova control modules to send position and velocity commands to the robot using kinova_msgs""" # The position action server definition and client self.pose_action_server = '/j2s7s300_driver/pose_action/tool_pose' self.pose_action_client = actionlib.SimpleActionClient(self.pose_action_server, ArmPoseAction) # Wait for client to be active self.pose_action_client.wait_for_server() # Create a arm pose goal for postion action client self.pose_goal = ArmPoseGoal() self.pose_goal.header = Header(frame_id='j2s7s300_link_base') # Velocity command topic definition self.velocity_topic = '/j2s7s300_driver/in/cartesian_velocity' # Velocity message self.velocity = PoseVelocity() # Create the velocity publisher self.velocity_publisher = rospy.Publisher(self.velocity_topic, PoseVelocity, queue_size=1) # Define a velocity publishing rate self.rate = rospy.Rate(100) #Hz
def cmmnd_CartesianVelocity(self, cart_velo):
msg = PoseVelocity(twist_linear_x=cart_velo[0],
twist_linear_y=cart_velo[1],
twist_linear_z=cart_velo[2],
twist_angular_x=cart_velo[3],
twist_angular_y=cart_velo[4],
twist_angular_z=cart_velo[5])
self.velocity_pub.publish(msg)
def cmmnd_CartesianVelocity(self, cart_velo):
msg = PoseVelocity(twist_linear_x=cart_velo[0],
twist_linear_y=cart_velo[1],
twist_linear_z=cart_velo[2],
twist_angular_x=cart_velo[3],
twist_angular_y=cart_velo[4],
twist_angular_z=cart_velo[5])
# rate = rospy.Rate(100)
# while not rospy.is_shutdown():
self.velocity_pub.publish(msg)
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 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 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 create_pose_velocity_msg(self,cart_velo,transform=False):
if transform:
if self.listen.frameExists("/root") and self.listen.frameExists("/j2n6a300_end_effector"):
self.listen.waitForTransform('/root',"/j2n6a300_end_effector",rospy.Time(),rospy.Duration(100.0))
t = self.listen.getLatestCommonTime("/root", "/j2n6a300_end_effector")
translation, quaternion = self.listen.lookupTransform("/root", "/j2n6a300_end_effector", t)
transform = self.transformer.fromTranslationRotation(translation, quaternion)
transformed_vel = np.dot(transform,(cart_velo[0:3]+[1.0]))
print(transformed_vel)
cart_velo[0:3] = transformed_vel[0:3]
msg = PoseVelocity(
twist_linear_x=cart_velo[0],
twist_linear_y=cart_velo[1],
twist_linear_z=cart_velo[2],
twist_angular_x=cart_velo[3],
twist_angular_y=cart_velo[4],
twist_angular_z=cart_velo[5])
return msg
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)
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 __init__(self):
self._kinova_pose_init = Pose()
self._sub = rospy.Subscriber("left_arm_driver/out/finger_position",
FingerPosition,
self.leftFingerPositionInit)
self._sub = rospy.Subscriber("right_arm_driver/out/finger_position",
FingerPosition,
self.rightFingerPositionInit)
self._sub = rospy.Subscriber("keyboard/keydown", Key,
self.KeyboradDownCallBack)
self._sub = rospy.Subscriber("keyboard/keyup", Key,
self.KeyboradUpCallBack)
self._pub_cmd_vel = rospy.Publisher("cmd_vel", Twist, queue_size=1)
self._pub_left_pose_velocity = rospy.Publisher(
"left_arm_driver/in/cartesian_velocity",
PoseVelocity,
queue_size=1)
self._pub_left_joint_velocity = rospy.Publisher(
'left_arm_driver/in/joint_velocity', JointVelocity, queue_size=1)
self._pub_right_pose_velocity = rospy.Publisher(
"right_arm_driver/in/cartesian_velocity",
PoseVelocity,
queue_size=1)
self._pub_right_joint_velocity = rospy.Publisher(
'right_arm_driver/in/joint_velocity', JointVelocity, queue_size=1)
self._pub_finger_position_left = rospy.Publisher(
'left_arm_driver/fingers_action/finger_positions/goal',
SetFingersPositionActionGoal,
queue_size=10)
self._pub_finger_position_right = rospy.Publisher(
'right_arm_driver/fingers_action/finger_positions/goal',
SetFingersPositionActionGoal,
queue_size=10)
self._finger_left = actionlib.SimpleActionClient(
'left_arm_driver/fingers_action/finger_positions',
SetFingersPositionAction)
self._finger_right = actionlib.SimpleActionClient(
'left_arm_driver/fingers_action/finger_positions',
SetFingersPositionAction)
self._home_client_left = rospy.ServiceProxy(
'left_arm_driver/in/home_arm', HomeArm)
self._home_client_right = rospy.ServiceProxy(
'right_arm_driver/in/home_arm', HomeArm)
self._driver_stop_client_left = rospy.ServiceProxy(
'/left_arm_driver/in/stop', Stop)
self._driver_start_client_left = rospy.ServiceProxy(
'/left_arm_driver/in/start', Start)
self._driver_stop_client_right = rospy.ServiceProxy(
'/left_arm_driver/in/stop', Stop)
self._driver_start_client_right = rospy.ServiceProxy(
'/left_arm_driver/in/start', Start)
self.vel_x_base = 0
self.vel_z_base = 0
self.vel_scale_base = 0.2
self.vel_x_left = 0
self.vel_y_left = 0
self.vel_z_left = 0
self.vel_x_right = 0
self.vel_y_right = 0
self.vel_z_right = 0
self.vel_joint1_left = 0
self.vel_joint2_left = 0
self.vel_joint3_left = 0
self.vel_joint4_left = 0
self.vel_joint5_left = 0
self.vel_joint6_left = 0
self.vel_joint1_right = 0
self.vel_joint2_right = 0
self.vel_joint3_right = 0
self.vel_joint4_right = 0
self.vel_joint5_right = 0
self.vel_joint6_right = 0
self.finger_left = 0
self.finger_right = 0
self.control_mode_left = 0
self.control_mode_right = 0
self.vel_left = 1
self.vel_right = 1
self.joint_direction = 1
self.left_stop = True
self.right_stop = True
self.finger_stop = True
self.allstart = 0
self.driver_start = 0
self.left_joint_stop = True
self.right_joint_stop = True
self.base_stop = True
self.pose_vel_left = PoseVelocity()
self.pose_vel_right = PoseVelocity()
self.joint_vel_left = JointVelocity()
self.joint_vel_right = JointVelocity()
self.finger_positions_left = SetFingersPositionGoal()
self.finger_positions_right = SetFingersPositionGoal()
self.finger_positions_msg_left = SetFingersPositionActionGoal()
self.finger_positions_msg_right = SetFingersPositionActionGoal()
self.vel_cmd_msg = Twist()
