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( q=feedback["q"], dq=feedback["dq"], target=target, ) # get obstacle position from CoppeliaSim obs_x, obs_y, obs_z = interface.get_xyz("obstacle") # pylint: disable=W0632 # update avoidance system about obstacle position avoid.set_obstacles([[obs_x, obs_y, obs_z, 0.05]]) if moving_obstacle is True: obs_x = 0.125 + 0.25 * np.sin(obs_count)
ctrlr = Joint(robot_config, kp=50) # create interface and connect interface = CoppeliaSim(robot_config=robot_config, dt=0.005) interface.connect() # make the target an offset of the current configuration feedback = interface.get_feedback() target = feedback["q"] + np.ones(robot_config.N_JOINTS) * 0.3 # For CoppeliaSim files that have a shadow arm to show the target configuration # get joint handles for shadow names = ["joint%i_shadow" % ii for ii in range(robot_config.N_JOINTS)] joint_handles = [] for name in names: interface.get_xyz(name) # this loads in the joint handle joint_handles.append(interface.misc_handles[name]) # move shadow to target position interface.send_target_angles(target, joint_handles) # set up arrays for tracking end-effector and target position q_track = [] try: count = 0 print("\nSimulation starting...\n") while count < 1500: # get joint angle and velocity feedback feedback = interface.get_feedback()
ee_track = [] ee_angles_track = [] target_track = [] target_angles_track = [] try: count = 0 print("\nSimulation starting...\n") while 1: # get arm feedback feedback = interface.get_feedback() hand_xyz = robot_config.Tx("EE", feedback["q"]) target = np.hstack( [interface.get_xyz("target"), interface.get_orientation("target")] ) u = ctrlr.generate(q=feedback["q"], dq=feedback["dq"], target=target,) # apply the control signal, step the sim forward interface.send_forces(u) # track data ee_track.append(np.copy(hand_xyz)) ee_angles_track.append( transformations.euler_from_matrix( robot_config.R("EE", feedback["q"]), axes="rxyz" ) ) tmp_target = np.copy(hand_xyz[:3])
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, ) u_adapt = np.zeros(robot_config.N_JOINTS) u_adapt[1:3] = adapt.generate( input_signal=np.array([feedback["q"][1], feedback["q"][2]]), training_signal=np.array( [ctrlr.training_signal[1], ctrlr.training_signal[2]]), )
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( q=feedback["q"], dq=feedback["dq"], target=target, ) # get obstacle position from CoppeliaSim obs_x, obs_y, obs_z = interface.get_xyz("obstacle") # update avoidance system about obstacle position avoid.set_obstacles([[obs_x, obs_y, obs_z, 0.05]]) if moving_obstacle is True: obs_x = 0.125 + 0.25 * np.sin(obs_count)