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)