def experimental_inverse_kinematics(robot,
link,
pose,
null_space=False,
max_iterations=200,
tolerance=1e-3):
(point, quat) = pose
# https://github.com/bulletphysics/bullet3/blob/389d7aaa798e5564028ce75091a3eac6a5f76ea8/examples/SharedMemory/PhysicsClientC_API.cpp
# https://github.com/bulletphysics/bullet3/blob/c1ba04a5809f7831fa2dee684d6747951a5da602/examples/pybullet/examples/inverse_kinematics_husky_kuka.py
joints = get_joints(
robot) # Need to have all joints (although only movable returned)
movable_joints = get_movable_joints(robot)
current_conf = get_joint_positions(robot, joints)
# TODO: sample other values for the arm joints as the reference conf
min_limits = [get_joint_limits(robot, joint)[0] for joint in joints]
max_limits = [get_joint_limits(robot, joint)[1] for joint in joints]
#min_limits = current_conf
#max_limits = current_conf
#max_velocities = [get_max_velocity(robot, joint) for joint in joints] # Range of Jacobian
max_velocities = [10000] * len(joints)
# TODO: cannot have zero velocities
# TODO: larger definitely better for velocities
#damping = tuple(0.1*np.ones(len(joints)))
#t0 = time.time()
#kinematic_conf = get_joint_positions(robot, movable_joints)
for iterations in range(max_iterations): # 0.000863273143768 / iteration
# TODO: return none if no progress
if null_space:
kinematic_conf = p.calculateInverseKinematics(
robot,
link,
point,
quat,
lowerLimits=min_limits,
upperLimits=max_limits,
jointRanges=max_velocities,
restPoses=current_conf,
#jointDamping=damping,
)
else:
kinematic_conf = p.calculateInverseKinematics(
robot, link, point, quat)
if (kinematic_conf is None) or any(map(math.isnan, kinematic_conf)):
return None
set_joint_positions(robot, movable_joints, kinematic_conf)
link_point, link_quat = get_link_pose(robot, link)
if np.allclose(link_point, point, atol=tolerance) and np.allclose(
link_quat, quat, atol=tolerance):
#print(iterations)
break
else:
return None
if violates_limits(robot, movable_joints, kinematic_conf):
return None
#total_time = (time.time() - t0)
#print(total_time)
#print((time.time() - t0)/max_iterations)
return kinematic_conf
def __init__(self):
if not self.load_parameters(): sys.exit(1)
self._limb = baxter_interface.Limb(self._arm)
self._kin = baxter_kinematics(self._arm)
self._planner = PathPlanner('{}_arm'.format(self._arm))
rospy.on_shutdown(self.shutdown)
plan = self._planner.plan_to_joint_pos(
np.array([-0.6, -0.4, -0.5, 0.6, -0.4, 1.1, -0.5]))
self._planner.execute_plan(plan)
rospy.sleep(5)
################################## Tensorflow bullshit
if self._learning_bool:
# I DON'T KNOW HOW THESE WORK
#define placeholders
observation_space = spaces.Box(low=-100,
high=100,
shape=(21, ),
dtype=np.float32)
action_space = spaces.Box(low=-50,
high=50,
shape=(56, ),
dtype=np.float32)
self._x_ph, self._u_ph = core.placeholders_from_spaces(
observation_space, action_space)
#define actor critic
#TODO add in central way to accept arguments
self._pi, self._logp, self._logp_pi, self._v = core.mlp_actor_critic(
self._x_ph,
self._u_ph,
hidden_sizes=(128, 2),
action_space=action_space)
# POLY_ORDER = 2
# self._pi, self._logp, self._logp_pi, self._v = core.polynomial_actor_critic(
# self._x_ph, self._u_ph, POLY_ORDER, action_space=action_space)
#start up tensorflow graph
var_counts = tuple(core.count_vars(scope) for scope in [u'pi'])
print(u'\nYoyoyoyyoyo Number of parameters: \t pi: %d\n' %
var_counts)
self._tf_vars = [
v for v in tf.trainable_variables() if u'pi' in v.name
]
self._num_tf_vars = sum(
[np.prod(v.shape.as_list()) for v in self._tf_vars])
print("tf vars is of length: ", len(self._tf_vars))
print("total trainable vars: ", len(tf.trainable_variables()))
self._sess = tf.Session()
self._sess.run(tf.global_variables_initializer())
print("total trainable vars: ", len(tf.trainable_variables()))
self._last_time = rospy.Time.now().to_sec()
current_position = utils.get_joint_positions(self._limb).reshape(
(7, 1))
current_velocity = utils.get_joint_velocities(self._limb).reshape(
(7, 1))
self._last_x = np.vstack([current_position, current_velocity])
self._last_xbar = self._last_x
if self._learning_bool:
self._last_a = self._sess.run(self._pi,
feed_dict={
self._x_ph:
self.preprocess_state(
self._last_x).reshape(1, -1)
})
else:
self._last_a = np.zeros((1, 56))
#################################### Set Tensorflow from saved params
self._READ_PARAMS = self._is_test_set
PARAM_INDEX = 201 ## This is the index of the learned parameters that you'll use (which epoch)
if self._learning_bool:
if self._READ_PARAMS:
import dill
# Put ABSOLUTE path to location of learned_params.pkl here,
# then remove the NotImplementedError. (This will probably be the
# same as the PREFIX variable in data_collector.py)
# eg. PREFIX = "/home/cc/ee106a/fa19/staff/ee106a-taf/Desktop/data"
PREFIX = "/home/cc/ee106b/sp20/staff/ee106b-laa/Desktop/data/read_params"
# raise NotImplementedError
lp = dill.load(open(PREFIX + "/learned_params.pkl", "rb"))
print("Running training set using data from epoch number: " +
str(PARAM_INDEX))
param_list = []
for param in lp[PARAM_INDEX][1]:
p_msg = Parameters(param)
param_list.append(p_msg)
lp_msg = LearnedParameters(param_list)
self._sess.run(
tf.assign(
tf.get_default_graph().get_tensor_by_name(
'pi/log_std:0'), tf.zeros((56, ))))
self.params_callback(lp_msg)
self._last_a = self._sess.run(
self._pi,
feed_dict={
self._x_ph:
self.preprocess_state(self._last_x).reshape(1, -1)
})
##################################### Controller params
self._A = np.vstack([
np.hstack([np.zeros((7, 7)), np.eye(7)]),
np.hstack([np.zeros((7, 7)), np.zeros((7, 7))])
])
self._B = np.vstack([np.zeros((7, 7)), np.eye(7)])
Q = 0.2 * np.diag([
1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
1.0
])
R = np.eye(7)
def solve_lqr(A, B, Q, R):
P = solve_continuous_are(A, B, Q, R)
K = np.dot(np.dot(np.linalg.inv(R), B.T), P)
return K
self._K = solve_lqr(self._A, self._B, Q, R)
# self._Kp = 6*np.diag(np.array([1, 1, 1.5, 1.5, 1, 1, 1]))
# self._Kv = 5*np.diag(np.array([2, 2, 1, 1, 0.8, 0.3, 0.3]))
# self._Kp = 9*np.diag(np.array([4, 6, 4, 8, 1, 5, 1]))
# self._Kv = 5*np.diag(np.array([2, 3, 2, 4, 0.8, 1.5, 0.3]))
# WORKS WELL FOR TRAINING
self._Kp = 6 * np.diag(np.array([1, 1, 1, 1, 1, 1, 1]))
self._Kv = 6 * np.diag(np.array([1, 1, 1, 1, 1, 1, 1]))
# Tune down for testing except I didn't
# self._Kp = 5*np.diag(np.array([1, 1, 1, 1, 1, 1, 1]))
# self._Kv = 5*np.diag(np.array([1, 1, 1, 1, 1, 1, 1]))
if not self.register_callbacks(): sys.exit(1)
def ref_callback(self, msg):
# Get/log state data
ref = msg
dt = rospy.Time.now().to_sec() - self._last_time
self._last_time = rospy.Time.now().to_sec()
current_position = utils.get_joint_positions(self._limb).reshape(
(7, 1))
current_velocity = utils.get_joint_velocities(self._limb).reshape(
(7, 1))
current_state = State(current_position, current_velocity)
# get dynamics info
positions_dict = utils.joint_array_to_dict(current_position,
self._limb)
velocity_dict = utils.joint_array_to_dict(current_velocity, self._limb)
inertia = self._kin.inertia(positions_dict)
# coriolis = self._kin.coriolis(positions_dict, velocity_dict)[0][0]
# coriolis = np.array([float(coriolis[i]) for i in range(7)]).reshape((7,1))
coriolis = self._kin.coriolis(positions_dict, velocity_dict).reshape(
(7, 1))
# gravity_wrench = np.array([0,0,0.981,0,0,0]).reshape((6,1))
# gravity_jointspace = (np.matmul(self._kin.jacobian_transpose(positions_dict), gravity_wrench))
# gravity = (np.matmul(inertia, gravity_jointspace)).reshape((7,1))
gravity = 0.075 * self._kin.gravity(positions_dict).reshape((7, 1))
# Linear Control
error = np.array(ref.setpoint.position).reshape(
(7, 1)) - current_position
d_error = np.array(ref.setpoint.velocity).reshape(
(7, 1)) - current_velocity
# d_error = np.zeros((7,1))
error_stack = np.vstack([error, d_error])
v = np.matmul(self._Kv, d_error).reshape(
(7, 1)) + np.matmul(self._Kp, error).reshape(
(7, 1)) + np.array(ref.feed_forward).reshape((7, 1))
# v = np.array(ref.feed_forward).reshape((7,1))
# v = np.matmul(self._K, error_stack).reshape((7,1))
# v = np.zeros((7,1))
# print current_position
##### Nonlinear control
x = np.vstack([current_position, current_velocity])
xbar = np.vstack([
np.array(ref.setpoint.position).reshape((7, 1)),
np.array(ref.setpoint.velocity).reshape((7, 1))
])
if self._learning_bool:
a = self._sess.run(self._pi,
feed_dict={
self._x_ph:
self.preprocess_state(x).reshape(1, -1)
})
else:
a = np.zeros((1, 56))
m2, f2 = np.split(0.1 * a[0], [49])
m2 = m2.reshape((7, 7))
f2 = f2.reshape((7, 1))
K = np.hstack([self._Kp, self._Kv])
x_predict = np.matmul(expm((self._A - np.matmul(self._B, K))*dt), self._last_x) + \
np.matmul(np.matmul(self._B, K), self._last_xbar)*dt
t_msg = Transition()
t_msg.x = list(self._last_x.flatten())
t_msg.a = list(self._last_a.flatten())
t_msg.r = -np.linalg.norm(x - x_predict, 2)
if self._learning_bool and not self._is_test_set:
self._transitions_pub.publish(t_msg)
data = DataLog(current_state, ref.setpoint, t_msg)
self._last_x = x
self._last_xbar = xbar
self._last_a = a
self._data_pub.publish(data)
torque_lim = np.array([4, 4, 4, 4, 2, 2, 2]).reshape((7, 1))
torque = np.matmul(inertia + m2, v) + coriolis + gravity + f2
torque = np.clip(torque, -torque_lim, torque_lim).reshape((7, 1))
torque_dict = utils.joint_array_to_dict(torque, self._limb)
self._limb.set_joint_torques(torque_dict)
def get_pddlstream_problem(task, context, collisions=True):
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
plant = task.mbp
robot = task.robot
robot_name = get_model_name(plant, robot)
world = plant.world_frame() # mbp.world_body()
robot_joints = get_movable_joints(plant, robot)
robot_conf = Conf(robot_joints, get_configuration(plant, context, robot))
init = [
('Robot', robot_name),
('CanMove', robot_name),
('Conf', robot_name, robot_conf),
('AtConf', robot_name, robot_conf),
('HandEmpty', robot_name),
]
goal_literals = []
if task.reset_robot:
goal_literals.append(('AtConf', robot_name, robot_conf),)
for obj in task.movable:
obj_name = get_model_name(plant, obj)
obj_pose = Pose(plant, world, obj, get_world_pose(plant, context, obj))
init += [('Graspable', obj_name),
('Pose', obj_name, obj_pose),
('AtPose', obj_name, obj_pose)]
for surface in task.surfaces:
init += [('Stackable', obj_name, surface)]
for surface in task.surfaces:
surface_name = get_model_name(plant, surface.model_index)
if 'sink' in surface_name:
init += [('Sink', surface)]
if 'stove' in surface_name:
init += [('Stove', surface)]
for door in task.doors:
door_body = plant.tree().get_body(door)
door_name = door_body.name()
door_joints = get_parent_joints(plant, door_body)
door_conf = Conf(door_joints, get_joint_positions(door_joints, context))
init += [
('Door', door_name),
('Conf', door_name, door_conf),
('AtConf', door_name, door_conf),
]
for positions in [get_door_positions(door_body, DOOR_OPEN)]:
conf = Conf(door_joints, positions)
init += [('Conf', door_name, conf)]
if task.reset_doors:
goal_literals += [('AtConf', door_name, door_conf)]
for obj, transform in task.goal_poses.items():
obj_name = get_model_name(plant, obj)
obj_pose = Pose(plant, world, obj, transform)
init += [('Pose', obj_name, obj_pose)]
goal_literals.append(('AtPose', obj_name, obj_pose))
for obj in task.goal_holding:
goal_literals.append(('Holding', robot_name, get_model_name(plant, obj)))
for obj, surface in task.goal_on:
goal_literals.append(('On', get_model_name(plant, obj), surface))
for obj in task.goal_cooked:
goal_literals.append(('Cooked', get_model_name(plant, obj)))
goal = And(*goal_literals)
print('Initial:', init)
print('Goal:', goal)
stream_map = {
'sample-grasp': from_gen_fn(get_grasp_gen_fn(task)),
'plan-ik': from_gen_fn(get_ik_gen_fn(task, context, collisions=collisions)),
'plan-motion': from_fn(get_motion_fn(task, context, collisions=collisions)),
'plan-pull': from_gen_fn(get_pull_fn(task, context, collisions=collisions)),
'TrajPoseCollision': get_collision_test(task, context, collisions=collisions),
'TrajConfCollision': get_collision_test(task, context, collisions=collisions),
}
#stream_map = 'debug' # Runs PDDLStream with "placeholder streams" for debugging
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
#!/usr/bin/env python
import rospy
import baxter_interface
from baxter_pykdl import baxter_kinematics
import utils
import numpy as np
if __name__ == '__main__': \
rospy.init_node('pykdl_test')
limb = baxter_interface.Limb('right')
kin = baxter_kinematics('right')
print kin.forward_position_kinematics()
current_position = utils.get_joint_positions(limb).reshape((7, 1))
print kin.forward_position_kinematics(
joint_values=utils.joint_array_to_dict(current_position, limb))
def main():
# TODO: teleporting kuka arm
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
client = connect(use_gui=args.viewer)
add_data_path()
#planeId = p.loadURDF("plane.urdf")
table = p.loadURDF("table/table.urdf", 0, 0, 0, 0, 0, 0.707107, 0.707107)
box = create_box(.07, .05, .15)
# boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation)
#pr2 = p.loadURDF("pr2_description/pr2.urdf")
pr2 = p.loadURDF("pr2_description/pr2_fixed_torso.urdf")
#pr2 = p.loadURDF("/Users/caelan/Programs/Installation/pr2_drake/pr2_local2.urdf",)
#useFixedBase=0,)
#flags=p.URDF_USE_SELF_COLLISION)
#flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT)
#flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
#pr2 = p.loadURDF("pr2_drake/urdf/pr2_simplified.urdf", useFixedBase=False)
initially_colliding = get_colliding_links(pr2)
print len(initially_colliding)
origin = (0, 0, 0)
print p.getNumConstraints()
# TODO: no way of controlling the base position by itself
# TODO: PR2 seems to collide with itself frequently
# real_time = False
# p.setRealTimeSimulation(real_time)
# left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES]
# control_joints(pr2, left_joints, TOP_HOLDING_LEFT_ARM)
# while True:
# control_joints(pr2, left_joints, TOP_HOLDING_LEFT_ARM)
# if not real_time:
# p.stepSimulation()
# A CollisionMap robot allows the user to specify self-collision regions indexed by the values of two joints.
# GetRigidlyAttachedLinks
print pr2
# for i in range (10000):
# p.stepSimulation()
# time.sleep(1./240.)
#print get_joint_names(pr2)
print [get_joint_name(pr2, joint) for joint in get_movable_joints(pr2)]
print get_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME))
#open_gripper(pr2, joint_from_name(pr2, LEFT_GRIPPER))
#print get_joint_limits(pr2, joint_from_name(pr2, LEFT_GRIPPER))
#print get_joint_position(pr2, joint_from_name(pr2, LEFT_GRIPPER))
print self_collision(pr2)
"""
print p.getNumConstraints()
constraint = fixed_constraint(pr2, -1, box, -1) # table
p.changeConstraint(constraint)
print p.getNumConstraints()
print p.getConstraintInfo(constraint)
print p.getConstraintState(constraint)
p.stepSimulation()
raw_input('Continue?')
set_point(pr2, (-2, 0, 0))
p.stepSimulation()
p.changeConstraint(constraint)
print p.getConstraintInfo(constraint)
print p.getConstraintState(constraint)
raw_input('Continue?')
print get_point(pr2)
raw_input('Continue?')
"""
# TODO: would be good if we could set the joint directly
print set_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME), 0.2) # Updates automatically
print get_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME))
#return
left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES]
right_joints = [joint_from_name(pr2, name) for name in RIGHT_JOINT_NAMES]
print set_joint_positions(pr2, left_joints, TOP_HOLDING_LEFT_ARM) # TOP_HOLDING_LEFT_ARM | SIDE_HOLDING_LEFT_ARM
print set_joint_positions(pr2, right_joints, REST_RIGHT_ARM) # TOP_HOLDING_RIGHT_ARM | REST_RIGHT_ARM
print get_body_name(pr2)
print get_body_names()
# print p.getBodyUniqueId(pr2)
print get_joint_names(pr2)
#for joint, value in zip(LEFT_ARM_JOINTS, REST_LEFT_ARM):
# set_joint_position(pr2, joint, value)
# for name, value in zip(LEFT_JOINT_NAMES, REST_LEFT_ARM):
# joint = joint_from_name(pr2, name)
# #print name, joint, get_joint_position(pr2, joint), value
# print name, get_joint_limits(pr2, joint), get_joint_type(pr2, joint), get_link_name(pr2, joint)
# set_joint_position(pr2, joint, value)
# #print name, joint, get_joint_position(pr2, joint), value
# for name, value in zip(RIGHT_JOINT_NAMES, REST_RIGHT_ARM):
# set_joint_position(pr2, joint_from_name(pr2, name), value)
print p.getNumJoints(pr2)
jointId = 0
print p.getJointInfo(pr2, jointId)
print p.getJointState(pr2, jointId)
# for i in xrange(10):
# #lower, upper = BASE_LIMITS
# #q = np.random.rand(len(lower))*(np.array(upper) - np.array(lower)) + lower
# q = np.random.uniform(*BASE_LIMITS)
# theta = np.random.uniform(*REVOLUTE_LIMITS)
# quat = z_rotation(theta)
# print q, theta, quat, env_collision(pr2)
# #set_point(pr2, q)
# set_pose(pr2, q, quat)
# #p.getMouseEvents()
# #p.getKeyboardEvents()
# raw_input('Continue?') # Stalls because waiting for input
#
# # TODO: self collisions
# for i in xrange(10):
# for name in LEFT_JOINT_NAMES:
# joint = joint_from_name(pr2, name)
# value = np.random.uniform(*get_joint_limits(pr2, joint))
# set_joint_position(pr2, joint, value)
# raw_input('Continue?')
#start = (-2, -2, 0)
#set_base_values(pr2, start)
# #start = get_base_values(pr2)
# goal = (2, 2, 0)
# p.addUserDebugLine(start, goal, lineColorRGB=(1, 1, 0)) # addUserDebugText
# print start, goal
# raw_input('Plan?')
# path = plan_base_motion(pr2, goal)
# print path
# if path is None:
# return
# print len(path)
# for bq in path:
# set_base_values(pr2, bq)
# raw_input('Continue?')
# left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES]
# for joint in left_joints:
# print joint, get_joint_name(pr2, joint), get_joint_limits(pr2, joint), \
# is_circular(pr2, joint), get_joint_position(pr2, joint)
#
# #goal = np.zeros(len(left_joints))
# goal = []
# for name, value in zip(LEFT_JOINT_NAMES, REST_LEFT_ARM):
# joint = joint_from_name(pr2, name)
# goal.append(wrap_joint(pr2, joint, value))
#
# path = plan_joint_motion(pr2, left_joints, goal)
# print path
# for q in path:s
# set_joint_positions(pr2, left_joints, q)
# raw_input('Continue?')
print p.JOINT_REVOLUTE, p.JOINT_PRISMATIC, p.JOINT_FIXED, p.JOINT_POINT2POINT, p.JOINT_GEAR # 0 1 4 5 6
movable_joints = get_movable_joints(pr2)
print len(movable_joints)
for joint in xrange(get_num_joints(pr2)):
if is_movable(pr2, joint):
print joint, get_joint_name(pr2, joint), get_joint_type(pr2, joint), get_joint_limits(pr2, joint)
#joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES]
#set_joint_positions(pr2, joints, sample_joints(pr2, joints))
#print get_joint_positions(pr2, joints) # Need to print before the display updates?
# set_base_values(pr2, (1, -1, -np.pi/4))
# movable_joints = get_movable_joints(pr2)
# gripper_pose = get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK))
# print gripper_pose
# print get_joint_positions(pr2, movable_joints)
# p.addUserDebugLine(origin, gripper_pose[0], lineColorRGB=(1, 0, 0))
# p.stepSimulation()
# raw_input('Pre2 IK')
# set_joint_positions(pr2, left_joints, SIDE_HOLDING_LEFT_ARM) # TOP_HOLDING_LEFT_ARM | SIDE_HOLDING_LEFT_ARM
# print get_joint_positions(pr2, movable_joints)
# p.stepSimulation()
# raw_input('Pre IK')
# conf = inverse_kinematics(pr2, gripper_pose) # Doesn't automatically set configuraitons
# print conf
# print get_joint_positions(pr2, movable_joints)
# set_joint_positions(pr2, movable_joints, conf)
# print get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK))
# #print get_joint_positions(pr2, movable_joints)
# p.stepSimulation()
# raw_input('Post IK')
# return
# print pose_from_tform(TOOL_TFORM)
# gripper_pose = get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK))
# #gripper_pose = multiply(gripper_pose, TOOL_POSE)
# #gripper_pose = get_gripper_pose(pr2)
# for i, grasp_pose in enumerate(get_top_grasps(box)):
# grasp_pose = multiply(TOOL_POSE, grasp_pose)
# box_pose = multiply(gripper_pose, grasp_pose)
# set_pose(box, *box_pose)
# print get_pose(box)
# raw_input('Grasp {}'.format(i))
# return
torso = joint_from_name(pr2, TORSO_JOINT_NAME)
torso_point, torso_quat = get_link_pose(pr2, torso)
#torso_constraint = p.createConstraint(pr2, torso, -1, -1,
# p.JOINT_FIXED, jointAxis=[0] * 3, # JOINT_FIXED
# parentFramePosition=torso_point,
# childFramePosition=torso_quat)
create_inverse_reachability(pr2, box, table)
ir_database = load_inverse_reachability()
print len(ir_database)
return
link = link_from_name(pr2, LEFT_ARM_LINK)
point, quat = get_link_pose(pr2, link)
print point, quat
p.addUserDebugLine(origin, point, lineColorRGB=(1, 1, 0)) # addUserDebugText
raw_input('Continue?')
current_conf = get_joint_positions(pr2, movable_joints)
#ik_conf = p.calculateInverseKinematics(pr2, link, point)
#ik_conf = p.calculateInverseKinematics(pr2, link, point, quat)
min_limits = [get_joint_limits(pr2, joint)[0] for joint in movable_joints]
max_limits = [get_joint_limits(pr2, joint)[1] for joint in movable_joints]
max_velocities = [get_max_velocity(pr2, joint) for joint in movable_joints] # Range of Jacobian
print min_limits
print max_limits
print max_velocities
ik_conf = p.calculateInverseKinematics(pr2, link, point, quat, lowerLimits=min_limits,
upperLimits=max_limits, jointRanges=max_velocities, restPoses=current_conf)
value_from_joint = dict(zip(movable_joints, ik_conf))
print [value_from_joint[joint] for joint in joints]
#print len(ik_conf), ik_conf
set_joint_positions(pr2, movable_joints, ik_conf)
#print len(movable_joints), get_joint_positions(pr2, movable_joints)
print get_joint_positions(pr2, joints)
raw_input('Finish?')
p.disconnect()