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)