def get(self, cur_target, cur_pos, keys_pressed=None): ''' cur_target and cur_pos should be specified in workspace coordinates ''' vx, vz = super(CenterOutCursorGoalJointSpace2D, self).get(cur_target, cur_pos, keys_pressed) vy = 0 px, py, pz = cur_pos pos = np.array([px, py, pz]) - self.shoulder_anchor vel = np.array([vx, vy, vz]) # Convert to joint velocities from riglib.stereo_opengl import ik joint_pos, joint_vel = ik.inv_kin_2D(pos, self.link_lengths[0], self.link_lengths[1], vel) return joint_vel[0]['sh_vabd'], joint_vel[0]['el_vflex']
def get(self, cur_target, cur_pos, keys_pressed=None): ''' cur_target and cur_pos should be specified in workspace coordinates ''' vx, vz = super(CenterOutCursorGoalJointSpace2D, self).get(cur_target, cur_pos, keys_pressed) vy = 0 px, py, pz = cur_pos pos = np.array([px, py, pz]) - self.shoulder_anchor vel = np.array([vx, vy, vz]) # Convert to joint velocities from riglib.stereo_opengl import ik joint_pos, joint_vel = ik.inv_kin_2D(pos, self.link_lengths[0], self.link_lengths[1], vel) return joint_vel[0]['sh_vabd'], joint_vel[0]['el_vflex']
def sim_joint_traj(targets, durations, link_lengths, shoulder_anchor, DT=0.1, inter_target_delay=0.3): ''' Parameters ---------- targets: np.array of shape (N, 3) Array of 3D targets durations: np.array of shape (N,) Durations of each reach link_lengths: iterable, List (or array) of link lengths of the arm shoulder_anchor: np.array of shape (3,) Spatial location of most proximal joint in space DT: float, optional, default = 0.1 Amount of time in between each discrete sample, in seconds inter_target_delay: float, optional, default=0.3 Amount of time in between each "trial", i.e. how long to wait at each target Returns ------- vel: np.array of shape (T, 3) Velocity trajectories ''' kin = sim_endpt_traj(targets, durations) # calculate joint velocities from riglib.stereo_opengl import ik joint_angles, joint_vel = ik.inv_kin_2D(kin[:, 0:3] - shoulder_anchor, link_lengths[1], link_lengths[0], kin[:, 3:6]) joint_kin = np.hstack( [rec_to_normal(joint_angles), rec_to_normal(joint_vel)]) return joint_kin
def stuff(): # Initialize the particles; n_particles = 10 n_joints = planar_chain.n_joints q_start = np.array([np.random.uniform(-pi, pi), np.random.uniform(0, pi), np.random.uniform(-pi/2, pi/2), np.random.uniform(-pi/2, 10*pi/180)]) noise = 5*np.random.randn(3) noise[1] = 0 angles = ik.inv_kin_2D(target_pos + noise, 15., 25.) q_start_constr = np.array([-angles[0][1], -angles[0][3], 0, 0]) n_iter = 10 particles_q = np.tile(q_start_constr, [n_particles, 1]) particles_v = np.random.randn(n_particles, n_joints) cost_fn = lambda x: cost(x, q_start) gbest = particles_q.copy() gbestcost = np.array(map(cost_fn, gbest)) pbest = gbest[np.argmin(gbestcost)] pbestcost = cost_fn(pbest) min_limits = np.array([x[0] for x in planar_chain.joint_limits]) max_limits = np.array([x[1] for x in planar_chain.joint_limits]) min_limits = np.tile(min_limits, [n_particles, 1]) max_limits = np.tile(max_limits, [n_particles, 1]) start_time = time.time() for k in range(n_iter): # update positions of particles particles_q += particles_v # apply joint limits # particles_q = np.array(map(lambda x: planar_chain.apply_joint_limits(x)[0], particles_q)) min_viol = particles_q < min_limits max_viol = particles_q > max_limits particles_q[min_viol] = min_limits[min_viol] particles_q[max_viol] = max_limits[max_viol] # update the costs costs = np.array(map(cost_fn, particles_q)) # update the 'bests' gbest[gbestcost > costs] = particles_q[gbestcost > costs] gbestcost = map(cost_fn, gbest) pbest = gbest[np.argmin(gbestcost)] pbestcost = cost_fn(pbest) # update the velocity phi1 = 1#np.random.rand() phi2 = 1#np.random.rand() w=0.25 c1=0.5 c2=0.25 particles_v = w*particles_v + c1*phi1*(np.tile(pbest, [n_particles, 1]) - particles_q) + c2*phi2*(gbest - particles_q) if np.linalg.norm(planar_chain.endpoint_pos(pbest) - target_pos) < 0.5: break end_time = time.time() print "Runtime = %g" % (end_time-start_time) return pbest
if np.linalg.norm(planar_chain.endpoint_pos(pbest) - target_pos) < 0.5: break end_time = time.time() print "Runtime = %g" % (end_time-start_time) return pbest starting_pos = np.array([-5., 0, 5]) target_pos = starting_pos - shoulder_anchor q_start = planar_chain.random_sample() noise = 5*np.random.randn(3) noise[1] = 0 angles = ik.inv_kin_2D(target_pos + noise, 15., 25.) q_start_constr = np.array([-angles[0][1], -angles[0][3], 0, 0]) pbest = planar_chain.inverse_kinematics_pso(q_start_constr, target_pos, verbose=True, time_limit=1.) # cProfile.run('planar_chain.inverse_kinematics_pso(q_start_constr, target_pos)', timeunit=0.001) import cProfile, pstats, StringIO pr = cProfile.Profile(timeunit=0.001) pr.enable() planar_chain.inverse_kinematics_pso(q_start_constr, target_pos) pr.disable() s = StringIO.StringIO() sortby = 'time' ps = pstats.Stats(pr, stream=s).sort_stats(sortby)
n_targets = target_list.shape[0] # Initialize the kinematic chain chain = robot_arms.PlanarXZKinematicChain([15, 15, 5, 5]) chain.joint_limits = [(-pi, pi), (-pi, 0), (-pi / 2, pi / 2), (-pi / 2, 10 * pi / 180)] A, B, W = train.tentacle_2D_state_space.get_ssm_matrices() ### Initialize the state of the arm starting_pos_ps = starting_pos_ws - shoulder_anchor q_start = chain.random_sample() noise = 5 * np.random.randn(3) noise[1] = 0 angles = ik.inv_kin_2D(starting_pos_ps + noise, 15., 25.) q_start_constr = np.array([-angles[0][1], -angles[0][3], 0, 0]) joint_pos = chain.inverse_kinematics_pso(starting_pos_ps, q_start_constr, verbose=True, time_limit=1.0, n_particles=30) cursor_pos = chain.endpoint_pos(joint_pos) print("starting position") print(starting_pos_ps) print("error = %g" % np.linalg.norm(cursor_pos - starting_pos_ps)) assister = assist.TentacleAssist(ssm=train.tentacle_2D_state_space, kin_chain=chain)
n_targets = target_list.shape[0] # Initialize the kinematic chain chain = robot_arms.PlanarXZKinematicChain([15, 15, 5, 5]) chain.joint_limits = [(-pi, pi), (-pi, 0), (-pi/2, pi/2), (-pi/2, 10*pi/180)] A, B, W = train.tentacle_2D_state_space.get_ssm_matrices() ### Initialize the state of the arm starting_pos_ps = starting_pos_ws - shoulder_anchor q_start = chain.random_sample() noise = 5*np.random.randn(3) noise[1] = 0 angles = ik.inv_kin_2D(starting_pos_ps + noise, 15., 25.) q_start_constr = np.array([-angles[0][1], -angles[0][3], 0, 0]) joint_pos = chain.inverse_kinematics_pso(starting_pos_ps, q_start_constr, verbose=True, time_limit=1.0, n_particles=30) cursor_pos = chain.endpoint_pos(joint_pos) print "starting position" print starting_pos_ps print "error = %g" % np.linalg.norm(cursor_pos - starting_pos_ps) assister = assist.TentacleAssist(ssm=train.tentacle_2D_state_space, kin_chain=chain) q_start = chain.inverse_kinematics(np.array([5., 0., 5.]) - shoulder_anchor, verbose=True, n_particles=500, eps=0.05, n_iter=10) x_init = np.hstack([q_start, np.zeros_like(q_start), 1]) x_init = np.mat(x_init).reshape(-1, 1) x = [x_init] goal_calc = goal_calculators.PlanarMultiLinkJointGoal(train.tentacle_2D_state_space, shoulder_anchor, chain, multiproc=True, init_resp=x_init)