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)
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')