# 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(