def lift_right_foot():
global rf_start_position
global rf_start_orientation
wholemsg = WholeBodyTrajectoryRosMessage()
wholemsg.unique_id = uid()
wholemsg.left_hand_trajectory_message.robot_side = 0
wholemsg.right_hand_trajectory_message.robot_side = 1
wholemsg.left_arm_trajectory_message.robot_side = 0
wholemsg.right_arm_trajectory_message.robot_side = 1
wholemsg.left_foot_trajectory_message.robot_side = 0
wholemsg.right_foot_trajectory_message.robot_side = 1
wholemsg.right_foot_trajectory_message.unique_id = wholemsg.unique_id
wholemsg.right_foot_trajectory_message.execution_mode = wholemsg.right_foot_trajectory_message.OVERRIDE
zero = Vector3(0.0, 0.0, 0.0)
p1 = SE3TrajectoryPointRosMessage()
p1.time = .25
p1.position = deepcopy(rf_start_position)
p1.position.z += 0.05
p1.position.x += 0.2
p1.orientation = deepcopy(rf_start_orientation)
#p1.orientation.z += 0.2
p1.linear_velocity = zero
p1.angular_velocity = zero
p2 = SE3TrajectoryPointRosMessage()
p2.time = .5
p2.position = deepcopy(rf_start_position)
p2.position.z -= 0.01
p2.position.x += 0.4
p2.orientation = deepcopy(rf_start_orientation)
#p2.orientation.z += 0.2
p2.linear_velocity = zero
p2.angular_velocity = zero
wholemsg.right_foot_trajectory_message.taskspace_trajectory_points = [ p1, p2 ]
rospy.loginfo('JPW lifting right foot: uid' + str(wholemsg.unique_id))
BodyPublisher.publish(wholemsg)
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 getCurrentPosition(stepSide):
handstep = SE3TrajectoryPointRosMessage()
if stepSide == HandTrajectoryRosMessage.LEFT:
hand_frame = LEFT_HAND_FRAME_NAME
else:
hand_frame = RIGHT_HAND_FRAME_NAME
handWorld = tfBuffer.lookup_transform('world', hand_frame, rospy.Time())
handstep.orientation = handWorld.transform.rotation
handstep.position = handWorld.transform.translation
rospy.loginfo('current world position: {0}'.format(handstep.position))
return handstep
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 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 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 do_a_little_dance():
wholemsg = WholeBodyTrajectoryRosMessage()
wholemsg.unique_id = uid()
rhandWorld = tfBuffer.lookup_transform('world', 'rightPalm', rospy.Time())
lhandWorld = tfBuffer.lookup_transform('world', 'leftPalm', rospy.Time())
hand_pt1 = SE3TrajectoryPointRosMessage()
hand_pt1.time = 1.0
hand_pt1.position = deepcopy(rhandWorld.transform.translation)
hand_pt1.orientation = rhandWorld.transform.rotation
hand_pt1.position.x += 0.5
hand_pt1.position.y -= 0.25
hand_pt1.position.z += 0.5
hand_pt2 = SE3TrajectoryPointRosMessage()
hand_pt2.time = 2.0 # How long to take to move the arm
hand_pt2.position = deepcopy(rhandWorld.transform.translation)
hand_pt2.orientation = rhandWorld.transform.rotation
hand_pt2.position.x -= 0.5
hand_pt2.position.y -= 0.25
hand_pt2.position.z -= 0.5
hand_pt3 = SE3TrajectoryPointRosMessage()
hand_pt3.time = 3.0 # How long to take to move the arm
hand_pt3.position = deepcopy(rhandWorld.transform.translation)
hand_pt3.orientation = rhandWorld.transform.rotation
hand_pt3.position.x += 0.25
hand_pt3.position.y += 0.25
hand_pt3b = SE3TrajectoryPointRosMessage()
hand_pt3b.time = 4.0 # How long to take to move the arm
hand_pt3b.position = deepcopy(rhandWorld.transform.translation)
hand_pt3b.orientation = rhandWorld.transform.rotation
hand_pt3b.position.x -= 0.25
hand_pt3b.position.y += 0.25
hand_pt3b.position.z += 1
wholemsg.right_hand_trajectory_message.taskspace_trajectory_points = [hand_pt1, hand_pt2, hand_pt3, hand_pt3b]
hand_pt4 = SE3TrajectoryPointRosMessage()
hand_pt4.time = 1.0 # How long to take to move the arm
hand_pt4.position = deepcopy(lhandWorld.transform.translation)
hand_pt4.orientation = lhandWorld.transform.rotation
hand_pt4.position.x -= 0.5
hand_pt4.position.y += 0.25
hand_pt4.position.z -= 0.5
hand_pt5 = SE3TrajectoryPointRosMessage()
hand_pt5.time = 2.0 # How long to take to move the arm
hand_pt5.position = deepcopy(lhandWorld.transform.translation)
hand_pt5.orientation = lhandWorld.transform.rotation
hand_pt5.position.x += 0.5
hand_pt5.position.y += 0.25
hand_pt5.position.z += 0.5
hand_pt6 = SE3TrajectoryPointRosMessage()
hand_pt6.time = 3.0 # How long to take to move the arm
hand_pt6.position = deepcopy(lhandWorld.transform.translation)
hand_pt6.orientation = lhandWorld.transform.rotation
hand_pt6.position.x += 0.25
hand_pt6.position.y -= 0.25
hand_pt7 = SE3TrajectoryPointRosMessage()
hand_pt7.time = 4.0 # How long to take to move the arm
hand_pt7.position = deepcopy(lhandWorld.transform.translation)
hand_pt7.orientation = lhandWorld.transform.rotation
hand_pt7.position.x -= 0.25
hand_pt7.position.y += 0.25
hand_pt7.position.z += 1
wholemsg.left_hand_trajectory_message.taskspace_trajectory_points = [hand_pt4, hand_pt5, hand_pt6, hand_pt7]
wholemsg.right_hand_trajectory_message.base_for_control = wholemsg.right_hand_trajectory_message.WORLD
wholemsg.right_hand_trajectory_message.execution_mode = wholemsg.right_hand_trajectory_message.OVERRIDE
wholemsg.right_hand_trajectory_message.unique_id = wholemsg.unique_id
wholemsg.left_hand_trajectory_message.base_for_control = wholemsg.left_hand_trajectory_message.WORLD
wholemsg.left_hand_trajectory_message.execution_mode = wholemsg.left_hand_trajectory_message.OVERRIDE
wholemsg.left_hand_trajectory_message.unique_id = wholemsg.unique_id
wholemsg.left_hand_trajectory_message.robot_side = 0
wholemsg.right_hand_trajectory_message.robot_side = 1
wholemsg.left_arm_trajectory_message.robot_side = 0
wholemsg.right_arm_trajectory_message.robot_side = 1
wholemsg.left_foot_trajectory_message.robot_side = 0
wholemsg.right_foot_trajectory_message.robot_side = 1
#wholemsg.chest_trajectory_message
#wholemsg.pelvis_trajectory_message
rospy.loginfo('Positioning Body: uid' + str(wholemsg.unique_id))
BodyPublisher.publish(wholemsg)
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)
def lift_left_foot():
global lf_start_position
global lf_start_orientation
wholemsg = WholeBodyTrajectoryRosMessage()
wholemsg.unique_id = uid()
wholemsg.left_hand_trajectory_message.robot_side = 0
wholemsg.right_hand_trajectory_message.robot_side = 1
wholemsg.left_arm_trajectory_message.robot_side = 0
wholemsg.right_arm_trajectory_message.robot_side = 1
wholemsg.left_foot_trajectory_message.robot_side = 0
wholemsg.right_foot_trajectory_message.robot_side = 1
wholemsg.left_foot_trajectory_message.unique_id = wholemsg.unique_id
wholemsg.left_foot_trajectory_message.execution_mode = 0
zero = Vector3(0.0, 0.0, 0.0)
p3 = SE3TrajectoryPointRosMessage()
p3.time = 0.25
p3.position = deepcopy(lf_start_position)
p3.position.z += 0.05
p3.position.x += 0
p3.orientation = deepcopy(lf_start_orientation)
p3.linear_velocity = zero
p3.angular_velocity = zero
p3a = SE3TrajectoryPointRosMessage()
p3a.time = 0.45
p3a.position = deepcopy(lf_start_position)
p3a.position.z += 0.05
p3a.position.x += 0.3
p3a.orientation = deepcopy(lf_start_orientation)
p3a.linear_velocity = zero
p3a.angular_velocity = zero
p4 = SE3TrajectoryPointRosMessage()
p4.time = 0.65
p4.position = deepcopy(lf_start_position)
p4.position.z -= 0.01
p4.position.x += 0.3
p4.orientation = deepcopy(lf_start_orientation)
p4.linear_velocity = zero
p4.angular_velocity = zero
wholemsg.left_foot_trajectory_message.taskspace_trajectory_points = [ p3, p3a, p4 ]
rjoints = make_forward_arm(2.00, 1)
wholemsg.right_arm_trajectory_message.joint_trajectory_messages = rjoints
wholemsg.right_arm_trajectory_message.execution_mode = 0
wholemsg.right_arm_trajectory_message.unique_id = wholemsg.unique_id
ljoints = make_backward_arm(2.00, 0)
wholemsg.left_arm_trajectory_message.joint_trajectory_messages = ljoints
wholemsg.left_arm_trajectory_message.execution_mode = 0
wholemsg.left_arm_trajectory_message.unique_id = wholemsg.unique_id
rospy.loginfo('JPW lifting left foot: uid' + str(wholemsg.unique_id))
BodyPublisher.publish(wholemsg)
def lift_right_foot():
global rf_start_position
global rf_start_orientation
wholemsg = WholeBodyTrajectoryRosMessage()
wholemsg.unique_id = uid()
wholemsg.left_hand_trajectory_message.robot_side = 0
wholemsg.right_hand_trajectory_message.robot_side = 1
wholemsg.left_arm_trajectory_message.robot_side = 0
wholemsg.right_arm_trajectory_message.robot_side = 1
wholemsg.left_foot_trajectory_message.robot_side = 0
wholemsg.right_foot_trajectory_message.robot_side = 1
wholemsg.right_foot_trajectory_message.unique_id = wholemsg.unique_id
wholemsg.right_foot_trajectory_message.previous_message_id = wholemsg.unique_id -1
wholemsg.left_foot_trajectory_message.previous_message_id = wholemsg.unique_id -1
wholemsg.right_foot_trajectory_message.execution_mode = 0
wholemsg.left_foot_trajectory_message.execution_mode = 0
zero = Vector3(0.0, 0.0, 0.0)
p1 = SE3TrajectoryPointRosMessage()
p1.time = .45
p1.position = deepcopy(rf_start_position)
p1.position.z += 0.05
p1.position.x += 0
p1.orientation = deepcopy(rf_start_orientation)
p1.linear_velocity = zero
p1.angular_velocity = zero
p1a = SE3TrajectoryPointRosMessage()
p1a.time = .85
p1a.position = deepcopy(rf_start_position)
p1a.position.z += 0.05
p1a.position.x += 0.6
p1a.orientation = deepcopy(rf_start_orientation)
p1a.linear_velocity = zero
p1a.angular_velocity = zero
p2 = SE3TrajectoryPointRosMessage()
p2.time = .95
p2.position = deepcopy(rf_start_position)
p2.position.z -= 0.01
p2.position.x += 0.6
p2.orientation = deepcopy(rf_start_orientation)
p2.linear_velocity = zero
p2.angular_velocity = zero
wholemsg.right_foot_trajectory_message.taskspace_trajectory_points = [ p1, p1a, p2 ]
rjoints = make_backward_arm(2.0, 1)
wholemsg.right_arm_trajectory_message.joint_trajectory_messages = rjoints
wholemsg.right_arm_trajectory_message.execution_mode = 0
wholemsg.right_arm_trajectory_message.unique_id = wholemsg.unique_id
wholemsg.right_arm_trajectory_message.previous_message_id = wholemsg.unique_id-1
ljoints = make_forward_arm(2.0, 0)
wholemsg.left_arm_trajectory_message.joint_trajectory_messages = ljoints
wholemsg.left_arm_trajectory_message.execution_mode = 0
wholemsg.left_arm_trajectory_message.unique_id = wholemsg.unique_id
wholemsg.left_arm_trajectory_message.previous_message_id = wholemsg.unique_id-1
rospy.loginfo('JPW lifting right foot: uid' + str(wholemsg.unique_id))
BodyPublisher.publish(wholemsg)