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)