def __init__(self, link_radii=arm_radius, joint_radii=arm_radius, link_lengths=[15,15,5,5], joint_colors=arm_color,
link_colors=arm_color, base_loc=np.array([2., 0., -15]), **kwargs):
'''
Instantiate the graphics and the virtual arm for a kinematic chain
'''
num_joints = 2
self.num_joints = 2
self.link_radii = make_list(link_radii, num_joints)
self.joint_radii = make_list(joint_radii, num_joints)
self.link_lengths = make_list(link_lengths, num_joints)
self.joint_colors = make_list(joint_colors, num_joints)
self.link_colors = make_list(link_colors, num_joints)
self.curr_vecs = np.zeros([num_joints, 3]) #rows go from proximal to distal links
# set initial vecs to correct orientations (arm starts out vertical)
self.curr_vecs[0,2] = self.link_lengths[0]
self.curr_vecs[1:,0] = self.link_lengths[1:]
# Create links
self.links = []
for i in range(self.num_joints):
joint = Sphere(radius=self.joint_radii[i], color=self.joint_colors[i])
# The most distal link gets a tapered cylinder (for purely stylistic reasons)
if i < self.num_joints - 1:
link = Cylinder(radius=self.link_radii[i], height=self.link_lengths[i], color=self.link_colors[i])
else:
link = Cone(radius1=self.link_radii[-1], radius2=self.link_radii[-1]/2, height=self.link_lengths[-1], color=self.link_colors[-1])
link_i = Group((link, joint))
self.links.append(link_i)
link_offsets = [0] + self.link_lengths[:-1]
self.link_groups = [None]*self.num_joints
for i in range(self.num_joints)[::-1]:
if i == self.num_joints-1:
self.link_groups[i] = self.links[i]
else:
self.link_groups[i] = Group([self.links[i], self.link_groups[i+1]])
self.link_groups[i].translate(0, 0, link_offsets[i])
# Call the parent constructor
super(RobotArmGen3D, self).__init__([self.link_groups[0]], **kwargs)
# Instantiate the kinematic chain object
if self.num_joints == 2:
self.kin_chain = robot_arms.PlanarXZKinematicChain(link_lengths)
self.kin_chain.joint_limits = [(-pi,pi), (-pi,0)]
else:
self.kin_chain = robot_arms.PlanarXZKinematicChain(link_lengths)
# TODO the code below is (obviously) specific to a 4-joint chain
self.kin_chain.joint_limits = [(-pi,pi), (-pi,0), (-pi/2,pi/2), (-pi/2, 10*pi/180)]
self.base_loc = base_loc
self.translate(*self.base_loc, reset=True)
def manually_spec_config_target(nblocks=100):
'''
Read the list of starting configurations and targets from a file
'''
import pickle
configurations, target_locations = pickle.load(
open(
'/storage/task_data/TentacleMultiConfig/starting_config_data.pkl'
))
from riglib.bmi import robot_arms
from itertools import izip
kin_chain = robot_arms.PlanarXZKinematicChain(
link_lengths=[15, 15, 5, 5])
reach_origins = [
kin_chain.endpoint_pos(config) for config in configurations
]
targs = [
np.vstack([origin, terminus])
for origin, terminus in izip(reach_origins, target_locations)
]
n_trials_per_block = len(targs)
data = []
for k in range(nblocks):
inds = np.arange(n_trials_per_block)
np.random.shuffle(inds)
for i in inds:
data_i = (configurations[i], targs[i])
data.append(data_i)
return data
reload(robot_arms) import numpy as np import matplotlib.pyplot as plt import time from riglib.stereo_opengl import ik import cProfile pi = np.pi q = np.array([0, 90, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) * pi/180 q_sub = q[1::3] chain = robot_arms.KinematicChain([15, 15, 5, 5]) [t, allt] = chain.forward_kinematics(q); planar_chain = robot_arms.PlanarXZKinematicChain([15, 15, 5, 5]) [t, allt] = planar_chain.forward_kinematics(q_sub); # TODO check the sign for the finger joint limits inf = np.inf planar_chain.joint_limits = [(-pi, pi), (-pi, 0), (-pi/2, pi/2), (-pi/2, 10*pi/180)] # target_pos = np.array([10, 0, 10]) shoulder_anchor = np.array([2, 0, -15]) x_target_pos = (np.random.randn() - 0.5)*25 z_target_pos = (np.random.randn() - 0.5)*14 target_pos = np.array([x_target_pos, 0, z_target_pos]) - shoulder_anchor target_pos = np.array([-14.37130744, 0. , 22.97472612]) q = q_sub[:]
