def sendRightHandTrajectory():
msg = HandTrajectoryRosMessage()
msg.robot_side = HandTrajectoryRosMessage.RIGHT
msg = appendTrajectoryPointHand(msg, 3.0, CURL_FINGER)
msg.unique_ig = -1
rospy.loginf('publish hand trajectory')
handTrajectoryPublisher
def make_hand_trajectory(self, sidename, position, orientation, time = 1.0,
mode = HandTrajectoryRosMessage.OVERRIDE,
base_for_control = HandTrajectoryRosMessage.WORLD, previous_msg = None):
if sidename == 'left':
side = ArmTrajectoryRosMessage.LEFT
elif sidename == 'right':
side = ArmTrajectoryRosMessage.RIGHT
else:
rospy.logerr("Unknown side {}".format(sidename))
return
point = SE3TrajectoryPointRosMessage()
point.time = time
point.position = position
point.orientation = orientation
msg = HandTrajectoryRosMessage()
msg.robot_side = side
msg.base_for_control = base_for_control
msg.taskspace_trajectory_points = [ point ]
msg.execution_mode = mode
msg.unique_id = self.zarj.uid
if previous_msg is not None:
msg.previous_message_id = previous_msg
return msg
def sendRightHandTrajectory():
handStart = getCurrentPosition(HandTrajectoryRosMessage.RIGHT)
# This message commands the controller to move in taskspace a hand to the desired pose (position &
# orientation) while going through the specified trajectory points. A third order polynomial function
# is used to interpolate positions and a hermite based curve (third order) is used to interpolate the
# orientations. To excute a single straight line trajectory to reach a desired hand pose, set only one
# trajectory point with zero velocity and its time to be equal to the desired trajectory time.
msg = HandTrajectoryRosMessage()
msg.robot_side = HandTrajectoryRosMessage.RIGHT
msg.base_for_control = HandTrajectoryRosMessage.CHEST
msg.execution_mode = HandTrajectoryRosMessage.OVERRIDE
msg.unique_id = rospy.Time.now().nsecs
trajectory = createHandOffset(HandTrajectoryRosMessage.RIGHT, [0.0, -0.2, -0.2])
trajectory.time = 1
msg.taskspace_trajectory_points.append(trajectory)
rospy.loginfo('publishing first right hand trajectory')
handTrajectoryPublisher.publish(msg)
time.sleep(5)
# trajectory = createHandOffset(HandTrajectoryRosMessage.RIGHT, [0.0, 0.2, 0.2])
trajectory.position = handStart.position
trajectory.orientation = handStart.orientation
trajectory.time = 1
msg.previous_message_id = msg.unique_id
msg.unique_id = rospy.Time.now().nsecs
msg.execution_mode = HandTrajectoryRosMessage.QUEUE
msg.taskspace_trajectory_points[0] = trajectory
rospy.loginfo('publishing second right hand trajectory')
handTrajectoryPublisher.publish(msg)
time.sleep(5)
def mashButton():
# Button pressed position gathered from Gazebo analysis
#buttonPosition = Vector3(3.35, -0.73, 1.25)
# we can subtract the length of the fingers... about 9 cm
buttonPosition = Vector3(3.16, -0.73, 1.25)
zarj_tf.transform_from_world(buttonPosition)
handWorld = zarj_tf.lookup_frame_in_world('rightPalm', rospy.Time())
hp1 = SE3TrajectoryPointRosMessage()
hp1.time = 0.2
hp1.position = deepcopy(handWorld.transform.translation)
hp1.position.y = buttonPosition.y
hp1.position.z = buttonPosition.z
hp1.orientation = handWorld.transform.rotation
hp2 = SE3TrajectoryPointRosMessage()
hp2.time = 0.3
hp2.position.x = buttonPosition.x
hp2.position.y = buttonPosition.y
hp2.position.z = buttonPosition.z
hp2.orientation = handWorld.transform.rotation
hp3 = SE3TrajectoryPointRosMessage()
hp3.time = 0.6
hp3.position = deepcopy(handWorld.transform.translation)
hp3.position.y = buttonPosition.y
hp3.position.z = buttonPosition.z
hp3.orientation = handWorld.transform.rotation
hp4 = SE3TrajectoryPointRosMessage()
hp4.time = 0.8
hp4.position = deepcopy(handWorld.transform.translation)
hp4.orientation = handWorld.transform.rotation
msg = HandTrajectoryRosMessage()
msg.robot_side = msg.RIGHT
msg.base_for_control = msg.WORLD
msg.execution_mode = msg.OVERRIDE
msg.taskspace_trajectory_points = [ hp1, hp2, hp3, hp4 ]
msg.unique_id = uid()
rospy.loginfo('AS Mash Button: uid' + str(msg.unique_id))
handTrajectoryPublisher.publish(msg)
def HandMsgMaker(time, side, POSITION_COORD, ORIENTATION_COORD):
testHand = HandTrajectoryRosMessage()
testHand.robot_side = side
testHand.unique_id = -1
testHand.previous_message_id = 0
testHand.execution_mode = 0
testHand.base_for_control = HandTrajectoryRosMessage.WORLD #Control with respect to world
testHand.taskspace_trajectory_points = [
copy.deepcopy(SE3TrajectoryPointRosMessage()) for i in range(1)
]
testHand.taskspace_trajectory_points[i].time = time
testHand.taskspace_trajectory_points[i].position.x = POSITION_COORD[0]
testHand.taskspace_trajectory_points[i].position.y = POSITION_COORD[1]
testHand.taskspace_trajectory_points[i].position.z = POSITION_COORD[2]
testHand.taskspace_trajectory_points[i].orientation.x = ORIENTATION_COORD[
0]
testHand.taskspace_trajectory_points[i].orientation.y = ORIENTATION_COORD[
1]
testHand.taskspace_trajectory_points[i].orientation.z = ORIENTATION_COORD[
2]
testHand.taskspace_trajectory_points[i].orientation.w = ORIENTATION_COORD[
3]
hand_publisher.publish(testHand)
def sendTaskSpaceTrajectory():
msg = HandTrajectoryRosMessage()
msg.robot_side = HandTrajectoryRosMessage.RIGHT
msg.base_for_control = HandTrajectoryRosMessage.CHEST
pt = SE3TrajectoryPointRosMessage()
pt.time = 2.0
pt.position.x = 0.28
pt.position.y = -0.7
pt.position.z = 1.0
pt.orientation.w = 1.0
pt.unique_id = 255
msg.taskspace_trajectory_points.append(pt)
msg.execution_mode = HandTrajectoryRosMessage.OVERRIDE
msg.unique_id = 212
rospy.loginfo('Moving Hand in task space')
taskSpaceTrajectoryPublisher.publish(msg)
def publish_once_and_kill(self):
msg = HandTrajectoryRosMessage()
msg.robot_side = DESIRED_HAND #msg.RIGHT
msg.execution_mode = msg.OVERRIDE
msg.unique_id = 2
if ON_REAL_ROBOT:
# REAL ROBOT ONLY
print 'On real Robot'
msg.frame_information.trajectory_reference_frame_id = WORLD_FRAME_HASH
msg.frame_information.data_reference_frame_id = WORLD_FRAME_HASH
msg.use_custom_control_frame = False
# END REAL_ROBOT_ONLY
else:
base_for_control = msg.WORLD
se3_final = SE3TrajectoryPointRosMessage()
# Set Zero Velocity for Linear and Angular components
zero_vel = Vector3()
zero_vel.x = 0.0
zero_vel.y = 0.0
zero_vel.z = 0.0
print 'Initial Hand Pose:'
# Get Current Hand Position and Orientation
current_hand_pos, current_hand_ori = self.get_current_hand_se3(
DESIRED_HAND)
se3_final.time = 3.0 # Trajectory Time
se3_final.position.x = DESIRED_HAND_POS.x
se3_final.position.y = DESIRED_HAND_POS.y
se3_final.position.z = DESIRED_HAND_POS.z
se3_final.orientation.x = DESIRED_HAND_ORI.x
se3_final.orientation.y = DESIRED_HAND_ORI.y
se3_final.orientation.z = DESIRED_HAND_ORI.z
se3_final.orientation.w = DESIRED_HAND_ORI.w
se3_final.unique_id = msg.unique_id
se3_final.linear_velocity = zero_vel
se3_final.angular_velocity = zero_vel
#msg.taskspace_trajectory_points.append(se3_init)
msg.taskspace_trajectory_points.append(se3_final)
print 'Sending Hand Command in World Frame...'
hand_name = "Right"
if DESIRED_HAND == 0: # LEFT
hand_name = "Left"
print " ", hand_name, "Des Hand Pos (x,y,z)", (DESIRED_HAND_POS.x,
DESIRED_HAND_POS.y,
DESIRED_HAND_POS.z)
print " ", hand_name, "Des Ori (x,y,z,w)", (DESIRED_HAND_ORI.x,
DESIRED_HAND_ORI.y,
DESIRED_HAND_ORI.z,
DESIRED_HAND_ORI.w)
print "......Sending message......"
self.hand_traj_pub.publish(msg)
rospy.sleep(4)
print 'Resulting Hand Pose:'
# Get Resulting Hand Position and Orientation
current_hand_pos, current_hand_ori = self.get_current_hand_se3(
DESIRED_HAND)
print " Position (x,y,z) error: ", (DESIRED_HAND_POS.x -
current_hand_pos.x,
DESIRED_HAND_POS.y -
current_hand_pos.y,
DESIRED_HAND_POS.z -
current_hand_pos.z)
sys.exit()
def pushButton(self, in_msg):
rospy.loginfo("Button press starting.")
# This message commands the controller to move in taskspace a hand to the desired pose (position &
# orientation) while going through the specified trajectory points. A third order polynomial function
# is used to interpolate positions and a hermite based curve (third order) is used to interpolate the
# orientations. To excute a single straight line trajectory to reach a desired hand pose, set only one
# trajectory point with zero velocity and its time to be equal to the desired trajectory time.
msg = HandTrajectoryRosMessage()
hand_side = in_msg.side
if hand_side == self.LEFT:
msg.robot_side = HandTrajectoryRosMessage.LEFT
hand_frame = self.LEFT_HAND_FRAME_NAME
rospy.loginfo("Using left hand to press button.")
else:
msg.robot_side = HandTrajectoryRosMessage.RIGHT
hand_frame = self.RIGHT_HAND_FRAME_NAME
rospy.loginfo("Using right hand to press button.")
msg.base_for_control = HandTrajectoryRosMessage.WORLD
msg.execution_mode = HandTrajectoryRosMessage.OVERRIDE
msg.unique_id = rospy.Time.now().nsecs
trajectory = SE3TrajectoryPointRosMessage()
trajectory.position = in_msg.button_position
# do not delete these next two or rotation fails
trajectory.linear_velocity = Vector3(0, 0, 0)
trajectory.angular_velocity = Vector3(0, 0, 0)
'''
Set palm parallel to y-z world plane, with fingers going right (or left)
By default, hand in world frame has palm up (z), fingers forward (x)
To get right hand fingers oriented properly, rotate 90 degrees about z
(for left it would be -90)
Then, we rotate 90 degrees about y so palm faces wall
'''
y_axis_unit = (0, 1, 0)
z_axis_unit = (0, 0, 1)
if hand_side == self.LEFT:
q1 = tf.transformations.quaternion_about_axis(
-numpy.pi / 2, z_axis_unit)
else:
q1 = tf.transformations.quaternion_about_axis(
numpy.pi / 2, z_axis_unit)
q2 = tf.transformations.quaternion_about_axis(numpy.pi / 2,
y_axis_unit)
q = tf.transformations.quaternion_multiply(q1, q2)
# TESTING ONLY set to identity quaternion
# q = [0, 0, 0, 1]
# this orientation IS a quaternion, in message definition
trajectory.orientation.x = q[0]
trajectory.orientation.y = q[1]
trajectory.orientation.z = q[2]
trajectory.orientation.w = q[3]
rospy.loginfo(trajectory.orientation)
motion_time = in_msg.motion_time
if motion_time == None or motion_time <= 0:
trajectory.time = 1
else:
trajectory.time = float(motion_time)
msg.taskspace_trajectory_points.append(trajectory)
rospy.loginfo('publishing button push trajectory')
self.trajectoryPublisher.publish(msg)