def Cos(x):
return np.cos(x)
def Sin(x):
return np.sin(x)
if __name__ == '__main__':
q0 = [
-1.3773043, 1.40564879, 1.774009, -2.181041, -1.35430, 1.401228,
0.059601
]
x_new = Affine(0.803, 0.0, 0.333 - 0.107, 0, 0, 0)
y, z = [], []
for j2 in np.linspace(0.0, 2.9, 200):
null_space = NullSpaceHandling(1, j2)
q_new = Kinematics.inverse(x_new.vector(), q0, null_space)
elbow = Affine(Kinematics.forwardElbow(q_new)).translation()
# print('New joints: ', j2, elbow)
# print('elbow', elbow)
y.append(elbow[1])
z.append(elbow[2])
# y.append(elbow[0])
from argparse import ArgumentParser
from frankx import Affine, LinearRelativeMotion, Robot
if __name__ == '__main__':
parser = ArgumentParser()
parser.add_argument('--host',
default='172.16.0.2',
help='FCI IP of the robot')
args = parser.parse_args()
# Connect to the robot
robot = Robot(args.host)
robot.set_default_behavior()
robot.recover_from_errors()
# Reduce the acceleration and velocity dynamic
robot.set_dynamic_rel(0.15)
# Define and move forwards
way = Affine(0.0, 0.2, 0.0)
motion_forward = LinearRelativeMotion(way)
robot.move(motion_forward)
# And move backwards using the inverse motion
motion_backward = LinearRelativeMotion(way.inverse())
robot.move(motion_backward)
if __name__ == '__main__':
parser = ArgumentParser()
parser.add_argument('--host',
default='172.16.0.2',
help='FCI IP of the robot')
args = parser.parse_args()
# Connect to the robot
robot = Robot(args.host, repeat_on_error=False)
robot.set_default_behavior()
robot.recover_from_errors()
# Reduce the acceleration and velocity dynamic
robot.set_dynamic_rel(0.2)
joint_motion = JointMotion([
-1.811944, 1.179108, 1.757100, -2.14162, -1.143369, 1.633046, -0.432171
])
robot.move(joint_motion)
# Define and move forwards
motion_down = WaypointMotion([
Waypoint(Affine(0.0, 0.0, -0.12), -0.2, Waypoint.Relative),
Waypoint(Affine(0.08, 0.0, 0.0), 0.0, Waypoint.Relative),
Waypoint(Affine(0.0, 0.1, 0.0, 0.0), 0.0, Waypoint.Relative),
])
# You can try to block the robot now.
robot.move(motion_down)
robot.set_default_behavior()
robot.recover_from_errors()
# Reduce the acceleration and velocity dynamic
robot.set_dynamic_rel(0.2)
joint_motion = JointMotion([
-1.811944, 1.179108, 1.757100, -2.14162, -1.143369, 1.633046, -0.432171
])
robot.move(joint_motion)
# Define and move forwards
impedance_motion = ImpedanceMotion(200.0, 20.0)
robot_thread = robot.move_async(impedance_motion)
sleep(0.05)
initial_target = impedance_motion.target
print('initial target: ', initial_target)
sleep(0.5)
impedance_motion.target = Affine(y=0.15) * initial_target
print('set new target: ', impedance_motion.target)
sleep(2.0)
impedance_motion.finish()
robot_thread.join()
print('motion finished')
if __name__ == '__main__':
parser = ArgumentParser()
parser.add_argument('--host',
default='172.16.0.2',
help='FCI IP of the robot')
args = parser.parse_args()
robot = Robot(args.host)
gripper = robot.get_gripper()
robot.set_default_behavior()
robot.recover_from_errors()
robot.set_dynamic_rel(0.2)
home_left = Waypoint(Affine(0.480, -0.15, 0.40), 1.65)
home_right = Waypoint(Affine(0.450, 0.05, 0.35), 1.65)
motion = WaypointMotion([home_left], return_when_finished=False)
thread = robot.move_async(motion)
gripper_thread = gripper.move_unsafe_async(0.05)
for new_affine, new_width in [(home_right, 0.02), (home_left, 0.07),
(home_right, 0.0)]:
input('Press enter for new affine (also while in motion!)\n')
motion.set_next_waypoint(new_affine)
gripper_thread = gripper.move_unsafe_async(new_width)
input('Press enter to stop\n')
motion.finish()
from frankx import Affine, Comparison, JointMotion, LinearRelativeMotion, Measure, MotionData, Reaction, Robot
if __name__ == '__main__':
# Connect to the robot
robot = Robot("172.16.0.2")
robot.set_default_behavior()
robot.recover_from_errors()
# Reduce the acceleration and velocity dynamic
robot.set_dynamic_rel(0.2)
joint_motion = JointMotion([
-1.811944, 1.179108, 1.757100, -2.14162, -1.143369, 1.633046, -0.432171
])
robot.move(joint_motion)
# Define and move forwards
motion_down = LinearRelativeMotion(Affine(0.0, 0.0, -0.11), -0.2)
motion_down_data = MotionData().with_reaction(
Reaction(Measure.ForceZ, Comparison.Less, -5.0,
LinearRelativeMotion(Affine(0.0, 0.0, 0.002), 0.0, 2.0)))
# motion_down_data = MotionData().with_reaction(Reaction(Measure.ForceZ < -5.0, LinearRelativeMotion(Affine(0.0, 0.0, 0.002), 0.0, 2.0)))
robot.move(motion_down, motion_down_data)
default='172.16.0.2',
help='FCI IP of the robot')
args = parser.parse_args()
robot = Robot(args.host)
robot.set_default_behavior()
robot.recover_from_errors()
# Reduce the acceleration and velocity dynamic
robot.set_dynamic_rel(0.2)
joint_motion = JointMotion([
-1.811944, 1.179108, 1.757100, -2.14162, -1.143369, 1.633046, -0.432171
])
robot.move(joint_motion)
linear_motion = LinearRelativeMotion(Affine(z=-0.19), -0.3)
linear_motion_data = MotionData().with_reaction(
Reaction(Measure.ForceZ < -8.0, StopMotion())) # [N]
robot.move(linear_motion, linear_motion_data)
# Define and move forwards
impedance_motion = ImpedanceMotion(800.0, 80.0)
# BE CAREFUL HERE!
# Move forward in y-direction while applying a force in z direction
impedance_motion.add_force_constraint(z=-8.0) # [N]
impedance_motion.set_linear_relative_target_motion(Affine(y=0.1),
2.0) # [m], [s]
robot.move(impedance_motion)
from argparse import ArgumentParser
from frankx import Affine, JointMotion, LinearMotion, Robot
if __name__ == '__main__':
parser = ArgumentParser()
parser.add_argument('--host',
default='172.16.0.2',
help='FCI IP of the robot')
args = parser.parse_args()
# Connect to the robot
robot = Robot(args.host)
robot.set_default_behavior()
robot.recover_from_errors()
robot.set_dynamic_rel(0.15)
# Joint motion
robot.move(
JointMotion([
-1.811944, 1.179108, 1.757100, -2.14162, -1.143369, 1.633046,
-0.432171
]))
# Define and move forwards
camera_frame = Affine(y=0.05)
home_pose = Affine(0.480, 0.0, 0.40)
robot.move(camera_frame, LinearMotion(home_pose, 1.75))
from argparse import ArgumentParser
from frankx import Affine, JointMotion, LinearMotion, Robot
if __name__ == '__main__':
parser = ArgumentParser()
parser.add_argument('--host',
default='172.16.0.2',
help='FCI IP of the robot')
args = parser.parse_args()
# Connect to the robot
robot = Robot(args.host)
robot.set_default_behavior()
robot.recover_from_errors()
robot.set_dynamic_rel(0.15)
# Joint motion
robot.move(
JointMotion([
-1.811944, 1.179108, 1.757100, -2.14162, -1.143369, 1.633046,
-0.432171
]))
# Define and move forwards
camera = Affine(y=0.05)
home = Affine(0.480, -0.05, 0.40)
robot.move(camera, LinearMotion(home, 1.75))
import numpy as np
from frankx import Affine, LinearMotion, Robot
# Connect to the robot
robot = Robot("172.16.0.2")
robot.recover_from_errors()
robot.set_dynamic_rel(0.05)
# Define and move forwards
camera = Affine(-0.079, -0.0005, 0.011, 0.0, 0.0, 0.0)
# camera = Affine(-0.079, -0.0005, 0.011, -np.pi / 4, 0.0, -np.pi)
home = Affine(0.480, -0.025, 0.21, 0.0)
robot.move(camera, LinearMotion(home))
from argparse import ArgumentParser
from frankx import Affine, JointMotion, LinearRelativeMotion, Measure, MotionData, Reaction, Robot, StopMotion
if __name__ == '__main__':
parser = ArgumentParser()
parser.add_argument('--host', default='172.16.0.2', help='FCI IP of the robot')
args = parser.parse_args()
# Connect to the robot
robot = Robot(args.host, repeat_on_error=False)
robot.set_default_behavior()
robot.recover_from_errors()
# Reduce the acceleration and velocity dynamic
robot.set_dynamic_rel(0.2)
joint_motion = JointMotion([-1.811944, 1.179108, 1.757100, -2.14162, -1.143369, 1.633046, -0.432171])
robot.move(joint_motion)
# Define and move forwards
motion_down = LinearRelativeMotion(Affine(0.0, 0.0, -0.12), -0.2)
motion_down_data = MotionData().with_reaction(Reaction(Measure.ForceZ < -5.0, StopMotion(Affine(0.0, 0.0, 0.002), 0.0)))
# You can try to block the robot now.
robot.move(motion_down, motion_down_data)
if motion_down_data.did_break:
print('Robot stopped at: ', robot.current_pose())
from frankx import Affine, Kinematics, NullSpaceHandling
if __name__ == '__main__':
# Joint configuration
q = [-1.45549, 1.15401, 1.50061, -2.30909, -1.3141, 1.9391, 0.02815]
# Forward kinematic
x = Affine(Kinematics.forward(q))
print('Current end effector position: ', x)
# Define new target position
x_new = Affine(x=0.1, y=0.0, z=0.0) * x
# Franka has 7 DoFs, so what to do with the remaining Null space?
null_space = NullSpaceHandling(2, 1.4) # Set elbow joint to 1.4
# Inverse kinematic with target, initial joint angles, and Null space configuration
q_new = Kinematics.inverse(x_new.vector(), q, null_space)
print('New position: ', x_new)
print('New joints: ', q_new)
import numpy as np
from frankx import Affine, LinearMotion, Robot
# Connect to the robot
robot = Robot("172.16.0.2")
robot.recover_from_errors()
robot.set_dynamic_rel(0.1)
# Define and move forwards
camera = Affine(-0.0005, 0.079, 0.011, 0.0, 0.0, 0.0)
gripper = Affine(0, 0, -0.18, 0.0, 0.0, 0.0)
home = Affine(0.480, -0.025, 0.21, 0.0)
robot.move(camera, LinearMotion(home, 1.6))
print(robot.current_pose(camera))
from argparse import ArgumentParser
from frankx import Affine, PathMotion, Robot
if __name__ == '__main__':
parser = ArgumentParser()
parser.add_argument('--host',
default='172.16.0.2',
help='FCI IP of the robot')
args = parser.parse_args()
# Connect to the robot
robot = Robot(args.host)
robot.set_default_behavior()
robot.recover_from_errors()
# Reduce the acceleration and velocity dynamic
robot.set_dynamic_rel(0.2)
# Define and move forwards
motion = PathMotion([
Affine(0.5, 0.0, 0.35),
Affine(0.5, 0.0, 0.24, -0.3),
Affine(0.5, -0.2, 0.35),
],
blend_max_distance=0.05)
robot.move(motion)