def tuck_arms(time):
wholemsg = WholeBodyTrajectoryRosMessage()
wholemsg.unique_id = uid()
rjoints = tuck_right_arm(time)
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 = tuck_left_arm(time)
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_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
rospy.loginfo('Tucking arm: uid' + str(wholemsg.unique_id))
BodyPublisher.publish(wholemsg)
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 = wholemsg.left_foot_trajectory_message.OVERRIDE
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.2
p3.orientation = deepcopy(lf_start_orientation)
#p3.orientation.z += 0.2
p3.linear_velocity = zero
p3.angular_velocity = zero
p4 = SE3TrajectoryPointRosMessage()
p4.time = 0.5
p4.position = deepcopy(lf_start_position)
p4.position.z -= 0.01
p4.position.x += 0.4
p4.orientation = deepcopy(lf_start_orientation)
#p4.orientation.z += 0.2
p4.linear_velocity = zero
p4.angular_velocity = zero
wholemsg.left_foot_trajectory_message.taskspace_trajectory_points = [
p3, p4
]
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.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 swing_arms():
global stage
wholemsg = WholeBodyTrajectoryRosMessage()
wholemsg.unique_id = uid()
stage = stage + 1
side = stage % 2
#Shoulder
if side:
rjoints = make_forward_arm(0.4, 1)
else:
rjoints = make_backward_arm(0.4, 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
#Shoulder
if side:
ljoints = make_backward_arm(0.4, 0)
else:
ljoints = make_forward_arm(0.4, 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_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 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 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)