# create our CoppeliaSim interface
interface = CoppeliaSim(robot_config, dt=0.001)
interface.connect()
# set up lists for tracking data
ee_track = []
target_track = []
try:
# get the end-effector's initial position
feedback = interface.get_feedback()
start = robot_config.Tx("EE", feedback["q"])
# make the target offset from that start position
target_xyz = start + np.array([0.2, -0.2, 0.0])
interface.set_xyz(name="target", xyz=target_xyz)
count = 0.0
print("\nSimulation starting...\n")
while count < 1500:
# get joint angle and velocity feedback
feedback = interface.get_feedback()
# calculate the control signal
u = ctrlr.generate(q=feedback["q"],
dq=feedback["dq"],
target=target_xyz)
# send forces into CoppeliaSim, step the sim forward
interface.send_forces(u)
target_track = []
# get Jacobians to each link for calculating perturbation
J_links = [
robot_config._calc_J("link%s" % ii, x=[0, 0, 0])
for ii in range(robot_config.N_LINKS)
]
try:
# get the end-effector's initial position
feedback = interface.get_feedback()
start = robot_config.Tx("EE", feedback["q"])
# make the target offset from that start position
target_xyz = start + np.array([0.2, -0.2, 0.0])
interface.set_xyz(name="target", xyz=target_xyz)
count = 0.0
while 1:
# get joint angle and velocity feedback
feedback = interface.get_feedback()
target = np.hstack(
[interface.get_xyz("target"),
interface.get_orientation("target")])
# calculate the control signal
u = ctrlr.generate(
q=feedback["q"],
dq=feedback["dq"],
target=target,
# set up lists for tracking data
ee_track = []
target_track = []
obstacle_track = []
moving_obstacle = True
obstacle_xyz = np.array([0.09596, -0.2661, 0.64204])
try:
# get visual position of end point of object
feedback = interface.get_feedback()
start = robot_config.Tx("EE", q=feedback["q"])
# make the target offset from that start position
target_xyz = start + np.array([0.2, -0.2, 0.0])
interface.set_xyz(name="target", xyz=target_xyz)
interface.set_xyz(name="obstacle", xyz=obstacle_xyz)
count = 0.0
obs_count = 0.0
print("\nSimulation starting...\n")
while count < 1500:
# get joint angle and velocity feedback
feedback = interface.get_feedback()
target = np.hstack(
[interface.get_xyz("target"),
interface.get_orientation("target")])
# calculate the control signal
u = ctrlr.generate(
traj_planner.generate_path(position=hand_xyz, target_position=target_position)
orientation_planner.match_position_path(
orientation=starting_orientation,
target_orientation=target_orientation,
position_path=traj_planner.position_path,
)
# set up lists for tracking data
ee_track = []
ee_angles_track = []
target_track = []
target_angles_track = []
try:
count = 0
interface.set_xyz("target", target_position)
interface.set_orientation(
"target",
transformations.euler_from_quaternion(target_orientation, axes="rxyz"))
print("\nSimulation starting...\n")
while 1:
# get arm feedback
feedback = interface.get_feedback()
hand_xyz = robot_config.Tx("EE", feedback["q"])
pos, vel = traj_planner.next()[:3]
orient = orientation_planner.next()
target = np.hstack([pos, orient])
u = ctrlr.generate(