args = parser.parse_args()
cfg = YamlConfig(args.cfg)
T_camera_ee = RigidTransform.load(cfg['T_camera_ee_path'])
T_camera_mount_delta = RigidTransform.load(cfg['T_camera_mount_path'])
logging.info('Starting robot')
fa = FrankaArm()
fa.set_tool_delta_pose(T_camera_mount_delta)
fa.reset_joints()
fa.open_gripper()
T_ready_world = fa.get_pose()
T_ready_world.translation[0] += 0.25
T_ready_world.translation[2] = 0.4
ret = fa.goto_pose(T_ready_world)
logging.info('Init camera')
sensor = get_first_realsense_sensor(cfg['rs'])
sensor.start()
logging.info('Detecting April Tags')
april = AprilTagDetector(cfg['april_tag'])
intr = sensor.color_intrinsics
T_tag_camera = april.detect(sensor, intr, vis=cfg['vis_detect'])[0]
T_camera_world = T_ready_world * T_camera_ee
T_tag_world = T_camera_world * T_tag_camera
logging.info('Tag has translation {}'.format(T_tag_world.translation))
logging.info('Finding closest orthogonal grasp')
T_grasp_world = get_closest_grasp_pose(T_tag_world, T_ready_world)
print('Opening gripper all the way')
fa.open_gripper()
# joint controls
print('Rotating last joint')
joints = fa.get_joints()
joints[6] += np.deg2rad(45)
fa.goto_joints(joints)
joints[6] -= np.deg2rad(45)
fa.goto_joints(joints)
# end-effector pose control
print('Translation')
T_ee_world = fa.get_pose()
T_ee_world.translation += [0.1, 0, 0.1]
fa.goto_pose(T_ee_world)
T_ee_world.translation -= [0.1, 0, 0.1]
fa.goto_pose(T_ee_world)
print('Rotation in end-effector frame')
T_ee_rot = RigidTransform(
rotation=RigidTransform.x_axis_rotation(np.deg2rad(45)),
from_frame='franka_tool', to_frame='franka_tool'
)
T_ee_world_target = T_ee_world * T_ee_rot
fa.goto_pose(T_ee_world_target)
fa.goto_pose(T_ee_world)
# reset franka back to home
fa.reset_joints()
from frankapy import FrankaArm
if __name__ == '__main__':
print('Starting robot')
fa = FrankaArm()
fa.reset_joints()
pose = fa.get_pose()
pose.translation[0] = 0.75
# This should trigger an error
fa.goto_pose(pose)
import rospy
if __name__ == "__main__":
fa = FrankaArm()
fa.reset_joints()
rospy.loginfo('Generating Trajectory')
p0 = fa.get_pose()
p1 = p0.copy()
T_delta = RigidTransform(translation=np.array([0, 0, 0.2]),
rotation=RigidTransform.z_axis_rotation(
np.deg2rad(30)),
from_frame=p1.from_frame,
to_frame=p1.from_frame)
p1 = p1 * T_delta
fa.goto_pose(p1)
T = 5
dt = 0.02
ts = np.arange(0, T, dt)
weights = [min_jerk_weight(t, T) for t in ts]
pose_traj = [p1.interpolate_with(p0, w) for w in weights]
z_stiffness_traj = [min_jerk(100, 800, t, T) for t in ts]
rospy.loginfo('Initializing Sensor Publisher')
pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC,
SensorDataGroup,
queue_size=1000)
rate = rospy.Rate(1 / dt)
from frankapy.proto import JointPositionSensorMessage, ShouldTerminateSensorMessage
from franka_interface_msgs.msg import SensorDataGroup
from frankapy.utils import min_jerk
import rospy
if __name__ == "__main__":
fa = FrankaArm()
fa.reset_joints()
rospy.loginfo('Generating Trajectory')
joints_0 = fa.get_joints()
p = fa.get_pose()
p.translation[2] -= 0.2
fa.goto_pose(p)
joints_1 = fa.get_joints()
T = 5
dt = 0.02
ts = np.arange(0, T, dt)
joints_traj = [min_jerk(joints_1, joints_0, t, T) for t in ts]
rospy.loginfo('Initializing Sensor Publisher')
pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC,
SensorDataGroup,
queue_size=1000)
rate = rospy.Rate(1 / dt)
rospy.loginfo('Publishing joints trajectory...')
# To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed
args = parser.parse_args()
cfg = YamlConfig(args.cfg)
T_camera_ee = RigidTransform.load(cfg['T_camera_ee_path'])
T_camera_mount_delta = RigidTransform.load(cfg['T_camera_mount_path'])
logging.info('Starting robot')
fa = FrankaArm()
fa.set_tool_delta_pose(T_camera_mount_delta)
fa.reset_joints()
fa.open_gripper()
T_ready_world = fa.get_pose()
T_ready_world.translation[0] += 0.25
T_ready_world.translation[2] = 0.4
fa.goto_pose(T_ready_world)
logging.info('Init camera')
sensor = get_first_realsense_sensor(cfg['rs'])
sensor.start()
logging.info('Detecting April Tags')
april = AprilTagDetector(cfg['april_tag'])
intr = sensor.color_intrinsics
T_tag_camera = april.detect(sensor, intr, vis=cfg['vis_detect'])[0]
T_camera_world = T_ready_world * T_camera_ee
T_tag_world = T_camera_world * T_tag_camera
logging.info('Tag has translation {}'.format(T_tag_world.translation))
logging.info('Finding closest orthogonal grasp')
T_grasp_world = get_closest_grasp_pose(T_tag_world, T_ready_world)