Exemple #1
0
                      training=False)

    # fa.run_guide_mode(15)
    # fa.goto_gripper(0.025, grasp=True)

    goal = goal2worldTF.translation
    goal[2] += 0.03  # offset the height
    goal[0] += 0.01

    print('Goal Loc = {}'.format(goal))
    current_pose = fa.get_pose().translation
    print('Current Loc = {}'.format(current_pose))
    goal_pose = (goal - current_pose).tolist()
    print('Sensor Value = {}'.format(goal_pose))

    # scaling factor for z trajectory
    goal_pose.append(-0.5)
    print(goal_pose)

    input("Press Enter to continue...")
    print('Running the dmp')
    # fa.run_guide_mode(args.time)
    fa.execute_pose_dmp(pose_dmp_info,
                        duration=args.time,
                        use_goal_formulation=False,
                        initial_sensor_values=goal_pose,
                        position_only=True,
                        joint_impedances=gains)

    fa.goto_gripper(0.08, grasp=False)
Exemple #2
0
import numpy as np
import math
import rospy
import argparse
import pickle
from autolab_core import RigidTransform, Point
from frankapy import FrankaArm

if __name__ == '__main__':
    parser = argparse.ArgumentParser()
    parser.add_argument('--pose_dmp_weights_file_path', '-f', type=str)
    args = parser.parse_args()

    print('Starting robot')
    fa = FrankaArm()

    pose_dmp_file = open(args.pose_dmp_weights_file_path, "rb")
    pose_dmp_info = pickle.load(pose_dmp_file)

    fa.execute_pose_dmp(pose_dmp_info, duration=4.2)
    parser = argparse.ArgumentParser()
    parser.add_argument('--pose_dmp_weights_file_path', type=str, default='')
    args = parser.parse_args()

    if not args.pose_dmp_weights_file_path:
        args.pose_dmp_weights_file_path = '/home/sony/logs/robot_state_data_0_pose_weights.pkl'

    pose_dmp_file = open(args.pose_dmp_weights_file_path, "rb")
    pose_dmp_info = pickle.load(pose_dmp_file)

    print('Starting robot')
    fa = FrankaArm()

    fa.reset_pose()

    fa.reset_joints()

    print('Opening Grippers')
    fa.open_gripper()

    # random_position = RigidTransform(rotation=np.array([
    #         [1, 0, 0],
    #         [0, -1, 0],
    #         [0, 0, -1]
    #     ]), translation=np.array([0.3069, 0, 0.2867]),
    # from_frame='franka_tool', to_frame='world')

    # fa.goto_pose_with_cartesian_control(random_position, 10)

    fa.execute_pose_dmp(pose_dmp_info, duration=10, skill_desc='pose_dmp')