def __init__(self):
dirname = os.path.dirname(os.path.abspath(__file__))
mujoco_env.MujocoEnv.__init__(
self, os.path.join(dirname, "mjc/baxter_orient_left_cts.xml"), 1)
utils.EzPickle.__init__(self)
## mujoco things
# task space action space
low = np.array([-1., -1.])
high = np.array([1., 1.])
self.action_space = spaces.Box(low, high)
self.tuck_pose = {
'left': np.array([-0.08, -1.0, -1.19, 1.94, 0.67, 1.03, -0.50])
}
self.start_pose = {
'left': np.array([-0.21, -0.75, -1.4, 1.61, 0.60, 0.81, -0.52])
}
## starting pose
self.init_qpos = self.data.qpos.copy().flatten()
self.init_qpos[1:8] = np.array(self.start_pose["left"]).T
## ik setup
urdf_filename = osp.join(dirname, "urdf", "baxter.urdf")
with open(urdf_filename) as f:
urdf = f.read()
# mode; Speed, Distance, Manipulation1, Manipulation2
self.ik_solver = TRAC_IK(
"base",
"left_gripper",
urdf,
0.005, # default seconds
1e-5, # default epsilon
"Speed")
self.old_state = np.zeros((9, ))
self.max_num_steps = 50
print("INIT DONE!")
def __init__(self):
# load in robot kinematic information
self.baselink = rospy.get_param("blue_hardware/baselink")
self.endlink = rospy.get_param("blue_hardware/endlink")
self.posture_target = rospy.get_param("blue_hardware/posture_target")
urdf = rospy.get_param("/robot_description")
# set up tf2 for transformation lookups
self.tf_buffer = tf2_ros.Buffer(
rospy.Duration(1.0)) # tf buffer length
tf_listener = tf2_ros.TransformListener(self.tf_buffer)
# build trac-ik solver
self.ik = TRAC_IK(self.baselink, self.endlink, urdf, 0.01, 5e-4,
"Distance")
# "Manipulation2")
# "Manipulation1")
rospy.Service('inverse_kinematics', InverseKinematics,
self.handle_ik_request)
from trac_ik_python.trac_ik_wrap import TRAC_IK
import rospy
from numpy.random import random
import time
if __name__ == '__main__':
# roslaunch pr2_description upload_pr2.launch
# Needed beforehand
urdf = rospy.get_param('/robot_description')
# params of constructor:
# base_link, tip_link, urdf_string, timeout, epsilon, solve_type="Speed"
# solve_type can be: Distance, Speed, Manipulation1, Manipulation2
ik_solver = TRAC_IK(
"torso_lift_link",
"r_wrist_roll_link",
urdf,
0.005, # default seconds
1e-5, # default epsilon
"Speed")
print("Number of joints:")
print(ik_solver.getNrOfJointsInChain())
print("Joint names:")
print(ik_solver.getJointNamesInChain(urdf))
print("Link names:")
print(ik_solver.getLinkNamesInChain())
qinit = [0.] * 7
x = y = z = 0.0
rx = ry = rz = 0.0
rw = 1.0
bx = by = bz = 0.001
def main():
urdf = rospy.get_param('/robot_description')
# print("urdf: ", urdf)
rospy.loginfo('Constructing IK solver...')
ik_solver = TRAC_IK("link0", "link7", urdf,
0.005, # default seconds
1e-5, # default epsilon
"Speed")
# print("Number of joints:")
# print(ik_solver.getNrOfJointsInChain())
# print("Joint names:")
# print(ik_solver.getJointNamesInChain(urdf))
# print("Link names:")
# print(ik_solver.getLinkNamesInChain())
qinit = [0.] * 7 #Initial status of the joints as seed.
x = y = z = 0.0
rx = ry = rz = 0.0
rw = 1.0
bx = by = bz = 0.001
brx = bry = brz = 0.1
# Generate a set of random coords in the arm workarea approx
NUM_COORDS = 200
rand_coords = []
for _ in range(NUM_COORDS):
x = random() * 0.5
y = random() * 0.6 + -0.3
z = random() * 0.7 + -0.35
rand_coords.append((x, y, z))
# Check some random coords with fixed orientation
avg_time = 0.0
num_solutions_found = 0
for x, y, z in rand_coords:
ini_t = time.time()
sol = ik_solver.CartToJnt(qinit,
x, y, z,
rx, ry, rz, rw,
bx, by, bz,
brx, bry, brz)
fin_t = time.time()
call_time = fin_t - ini_t
# print "IK call took: " + str(call_time)
avg_time += call_time
if sol:
# print "X, Y, Z: " + str( (x, y, z) )
print "SOL: " + str(sol)
num_solutions_found += 1
avg_time = avg_time / NUM_COORDS
print
print "Found " + str(num_solutions_found) + " of 200 random coords"
print "Average IK call time: " + str(avg_time)
print
# Check if orientation bounds work
avg_time = 0.0
num_solutions_found = 0
brx = bry = brz = 9999.0 # We don't care about orientation
for x, y, z in rand_coords:
ini_t = time.time()
sol = ik_solver.CartToJnt(qinit,
x, y, z,
rx, ry, rz, rw,
bx, by, bz,
brx, bry, brz)
fin_t = time.time()
call_time = fin_t - ini_t
# print "IK call took: " + str(call_time)
avg_time += call_time
if sol:
# print "X, Y, Z: " + str( (x, y, z) )
# print "SOL: " + str(sol)
num_solutions_found += 1
avg_time = avg_time / NUM_COORDS
print
print "Found " + str(num_solutions_found) + " of 200 random coords ignoring orientation"
print "Average IK call time: " + str(avg_time)
print
# Check if big position and orientation bounds work
avg_time = 0.0
num_solutions_found = 0
bx = by = bz = 9999.0
brx = bry = brz = 9999.0
for x, y, z in rand_coords:
ini_t = time.time()
sol = ik_solver.CartToJnt(qinit,
x, y, z,
rx, ry, rz, rw,
bx, by, bz,
brx, bry, brz)
fin_t = time.time()
call_time = fin_t - ini_t
# print "IK call took: " + str(call_time)
avg_time += call_time
if sol:
# print "X, Y, Z: " + str( (x, y, z) )
# print "SOL: " + str(sol)
num_solutions_found += 1
avg_time = avg_time / NUM_COORDS
print
print "Found " + str(num_solutions_found) + " of 200 random coords ignoring everything"
print "Average IK call time: " + str(avg_time)
print