def handle_compute_ik_divided(self, req):
nb_frames = len(req.desired_poses)
# convert the desired poses to dict
desired_dict = {}
for pose in req.desired_poses:
desired_dict[pose.header.frame_id] = transformations.pose_to_list(pose)
# convert the fixed joint state to dict
fixed_joints_dict = {}
for i in range(len(req.fixed_joints.name)):
fixed_joints_dict[req.fixed_joints.name[i]] = req.fixed_joints.position[i]
# create as many threads as nb frames
threads = [None] * nb_frames
results = {}
for i, frame in enumerate(desired_dict.keys()):
threads[i] = Thread(target=self.compute_sub_ik, args=(frame, desired_dict, results, req.tolerance))
threads[i].start()
# join thread results
for i in range(len(threads)):
threads[i].join()
# convert restult to a joint state
js = req.seed
js.position = list(js.position)
js.name = list(js.name)
# replace joint values with optimized ones
for key, value in results.iteritems():
for i, name in enumerate(self.joint_by_links[key]):
js.position[js.name.index(name)] = value[i]
# replace fixed values
for key, value in fixed_joints_dict.iteritems():
js.position[js.name.index(key)] = value
# return server reply
return GetHumanIKResponse(js)
def pathtodict(path):
assert isinstance(path, Path)
frame_id = path.header.frame_id
if frame_id == '' and len(path.poses) > 0:
frame_id = path.poses[0].header.frame_id
dic = {"frame_id": frame_id, "points": []}
for point in path.poses:
dic["points"].append({"time": point.header.stamp.to_sec(), "pose": pose_to_list(point)})
return dic
def rollout(self, goal):
assert isinstance(goal, PoseStamped)
self._dmp.goal = [val for sublist in pose_to_list(goal) for val in sublist]
y_track, dy_track, ddy_track = self._dmp.rollout()
path = Path()
for y in y_track:
path.poses.append(list_to_pose([[y[0], y[1], y[2]], [y[3], y[4], y[5], y[6]]],
frame_id=self.init_path.header.frame_id))
path.header.stamp = rospy.Time.now()
path.header.frame_id = self.init_path.header.frame_id
return path
def _path_to_y_des(self, path, nss):
y_des = []
for pose_s in path.poses:
assert pose_s.header.frame_id == self.init_path.header.frame_id
pose = [val for sublist in pose_to_list(pose_s) for val in sublist] # flatten to [x, y, z, x, y, z, w]
y_des.append(pose)
# Repeat the last point (stable state) n times to avoid brutal cuts due to asymptotic approach
for n in range(nss):
y_des.append(y_des[-1])
return np.array(y_des).transpose()
def _get_ik_pykdl(self, eef_poses, seeds=(), params=None):
solutions = []
for pose_num, eef_pose in enumerate(eef_poses):
if eef_pose.header.frame_id.strip('/') != self._world.strip('/'):
raise NotImplementedError("_get_ik_pykdl: Baxter PyKDL implementation does not accept frame_ids other than {}".format(self._world))
pose = pose_to_list(eef_pose)
resp = self._kinematics_pykdl.inverse_kinematics(pose[0], pose[1],
[seeds[pose_num].joint_state.position[seeds[pose_num].joint_state.name.index(joint)]
for joint in self.joint_names()] if len(seeds)>0 else None)
if resp is None:
rs = None
else:
rs = RobotState()
rs.is_diff = False
rs.joint_state.name = self.joint_names()
rs.joint_state.position = resp
solutions.append(rs)
return solutions
def handle_compute_ik(self, req):
joint_names_set = set()
# get the number of joints according to the required group
for g in req.group_names:
joint_names_set.update(self.model.get_joint_names(g))
self.joint_names = list(joint_names_set)
# convert the desired poses to dict
desired_dict = {}
for pose in req.desired_poses:
desired_dict[pose.header.frame_id] = transformations.pose_to_list(pose)
# convert the fixed joint state to dict
fixed_joints_dict = {}
for i in range(len(req.fixed_joints.name)):
fixed_joints_dict[req.fixed_joints.name[i]] = req.fixed_joints.position[i]
# get initial state
init_state = req.seed
# get only joints to optimize
init_joints = []
for name in self.joint_names:
init_joints.append(init_state.position[init_state.name.index(name)])
# get the joints limits for the optimization
joint_limits = self.model.get_joint_limits(self.joint_names)
# optimize to find the corresponding IK
res = minimize(self.cost_function, init_joints,
# jac=self.jacobian_cost_function,
args=(desired_dict, fixed_joints_dict, ),
method='L-BFGS-B',
bounds=joint_limits)
opt_joints = list(res.x)
# convert it to a joint state
js = self.model.get_current_state()
js.position = list(js.position)
js.name = list(js.name)
# replace joint values with optimized ones
for i in range(len(self.joint_names)):
name = self.joint_names[i]
js.position[js.name.index(name)] = opt_joints[i]
# replace fixed values
for key, value in fixed_joints_dict.iteritems():
js.position[js.name.index(key)] = value
# return server reply
return GetHumanIKResponse(js)
def add_demonstration(self, demonstration, js_demo):
"""
Add a new task-space demonstration and update the model
:param demonstration: Path object
:param js_demo: Joint space demo for duration TODO replace Path by something else including duration?
:return:
"""
if not isinstance(demonstration, Path):
raise TypeError("ros.TaskProMP.add_demonstration only accepts Path, got {}".format(type(demonstration)))
if isinstance(js_demo, RobotTrajectory):
js_demo = js_demo.joint_trajectory
elif not isinstance(js_demo, JointTrajectory):
raise TypeError("ros.ProMP.add_demo only accepts RT or JT, got {}".format(type(js_demo)))
self._durations.append(
js_demo.points[-1].time_from_start.to_sec() - js_demo.points[0].time_from_start.to_sec())
self.joint_names = js_demo.joint_names
demo_array = [list_to_raw_list(pose_to_list(pose)) for pose in demonstration.poses]
self.promp.add_demonstration(demo_array)
def _get_ik_pykdl(self, eef_poses, seeds=(), params=None):
solutions = []
for pose_num, eef_pose in enumerate(eef_poses):
if eef_pose.header.frame_id.strip('/') != self._world.strip('/'):
raise NotImplementedError(
"_get_ik_pykdl: Baxter PyKDL implementation does not accept frame_ids other than {}"
.format(self._world))
pose = pose_to_list(eef_pose)
resp = self._kinematics_pykdl.inverse_kinematics(
pose[0], pose[1], [
seeds[pose_num].joint_state.position[
seeds[pose_num].joint_state.name.index(joint)]
for joint in self.joint_names()
] if len(seeds) > 0 else None)
if resp is None:
rs = None
else:
rs = RobotState()
rs.is_diff = False
rs.joint_state.name = self.joint_names()
rs.joint_state.position = resp
solutions.append(rs)
return solutions
class HumanModel(object):
def __init__(self,
description='human_description',
prefix='human',
control=False):
rospack = RosPack()
self.path = rospack.get_path('human_moveit_config')
self.description = description
self.robot_commander = moveit_commander.RobotCommander(description)
if control:
self.joint_publisher = rospy.Publisher('/human/set_joint_values',
JointState,
queue_size=1)
self.groups = {}
self.groups['head'] = moveit_commander.MoveGroupCommander(
'Head', description)
self.groups['right_arm'] = moveit_commander.MoveGroupCommander(
'RightArm', description)
self.groups['left_arm'] = moveit_commander.MoveGroupCommander(
'LeftArm', description)
self.groups['right_leg'] = moveit_commander.MoveGroupCommander(
'RightLeg', description)
self.groups['left_leg'] = moveit_commander.MoveGroupCommander(
'LeftLeg', description)
self.groups['upper_body'] = moveit_commander.MoveGroupCommander(
'UpperBody', description)
self.groups['lower_body'] = moveit_commander.MoveGroupCommander(
'LowerBody', description)
self.groups['whole_body'] = moveit_commander.MoveGroupCommander(
'WholeBody', description)
# initialize end-effectors dict
self.end_effectors = {}
# fill both dict
for key, value in self.groups.iteritems():
self.end_effectors[key] = value.get_end_effector_link()
# add the list of end-effectors for bodies
self.end_effectors['upper_body'] = [
self.end_effectors['head'], self.end_effectors['right_arm'],
self.end_effectors['left_arm']
]
self.end_effectors['lower_body'] = [
self.end_effectors['right_leg'], self.end_effectors['left_leg']
]
self.end_effectors['whole_body'] = self.end_effectors[
'upper_body'] + self.end_effectors['lower_body']
# initialize common links per group
self.group_links = {}
self.group_links['head'] = [
prefix + '/shoulder_center', prefix + '/head'
]
# fill the disct of active joints by links
self.joint_by_links = {}
self.joint_by_links[prefix + '/shoulder_center'] = [
'spine_0', 'spine_1', 'spine_2'
]
self.joint_by_links[prefix + '/head'] = ['neck_0', 'neck_1', 'neck_2']
sides = ['right', 'left']
for s in sides:
self.group_links[s + '_arm'] = [
prefix + '/' + s + '_shoulder', prefix + '/' + s + '_elbow',
prefix + '/' + s + '_hand'
]
self.group_links[s + '_leg'] = [
prefix + '/' + s + '_hip', prefix + '/' + s + '_knee',
prefix + '/' + s + '_foot'
]
# arm links
self.joint_by_links[prefix + '/' + s + '_shoulder'] = [
s + '_shoulder_0', s + '_shoulder_1', s + '_shoulder_2'
]
self.joint_by_links[prefix + '/' + s +
'_elbow'] = [s + '_elbow_0', s + '_elbow_1']
self.joint_by_links[prefix + '/' + s +
'_hand'] = [s + '_wrist_0', s + '_wrist_1']
# leg links
self.joint_by_links[prefix + '/' + s + '_hip'] = [
s + '_hip_0', s + '_hip_1', s + '_hip_2'
]
self.joint_by_links[prefix + '/' + s + '_knee'] = [s + '_knee']
self.joint_by_links[prefix + '/' + s +
'_foot'] = [s + '_ankle_0', s + '_ankle_1']
self.prefix = prefix
rospy.wait_for_service('compute_fk')
self.compute_fk = rospy.ServiceProxy('compute_fk', GetPositionFK)
self.current_state = self.get_initial_state()
def init_nearest_neighbour_trees(self):
def load_database(link):
database = np.load(
join(self.path, 'database8', 'database_' + link + '.npz'))
data_dict = {}
data_dict['data'] = database
if self.with_orient:
data_dict['tree'] = cKDTree(database['data'][:, -7:])
else:
data_dict['tree'] = cKDTree(database['data'][:, -7:-4])
return data_dict
self.trees = {}
links = ['head', 'shoulder_center']
for s in ['right', 'left']:
links += [s + '_elbow', s + '_hand']
for l in links:
self.trees[l] = load_database(l)
def get_group_of_link(self, link):
for key, value in self.group_links.iteritems():
if link in value:
group = key
break
return group
def extract_group_joints(self, group_name, joint_state):
active = self.groups[group_name].get_active_joints()
res = []
for joint in active:
index = joint_state.name.index(joint)
res.append(joint_state.position[index])
return res
def forward_kinematic(self, joint_state, base='base', links=None):
def compute_fk_client():
try:
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = self.prefix + '/base'
rs = RobotState()
rs.joint_state = joint_state
res = self.compute_fk(header, links, rs)
return res.pose_stamped
except rospy.ServiceException, e:
print "Service call failed: %s" % e
# in case of troubles return 0 pose stamped
return []
if links is None:
links = self.end_effectors['whole_body']
if type(links) is not list:
if links == "all":
links = self.get_link_names('whole_body')
else:
links = [links]
# check that the base is in links
if base != 'base' and base not in links:
links.append(base)
pose_stamped_list = compute_fk_client()
if not pose_stamped_list:
return {}
# transform it in a dict of poses
pose_dict = {}
if base != 'base':
tr_base = transformations.pose_to_list(
pose_stamped_list[links.index(base)].pose)
inv_base = transformations.inverse_transform(tr_base)
for i in range(len(links)):
if links[i] != base:
tr = transformations.pose_to_list(
pose_stamped_list[i].pose)
pose_dict[links[i]] = transformations.multiply_transform(
inv_base, tr)
else:
for i in range(len(links)):
pose_dict[links[i]] = transformations.pose_to_list(
pose_stamped_list[i].pose)
return pose_dict
class HumanModel(object):
def __init__(self,
description='human_description',
prefix='human',
control=False):
self.description = description
self.robot_commander = RobotCommander(description)
if control:
self.joint_publisher = rospy.Publisher('/human/set_joint_values',
JointState,
queue_size=1)
self.groups = {}
self.groups['head'] = MoveGroupCommander('Head', description)
self.groups['right_arm'] = MoveGroupCommander('RightArm', description)
self.groups['left_arm'] = MoveGroupCommander('LeftArm', description)
self.groups['right_leg'] = MoveGroupCommander('RightLeg', description)
self.groups['left_leg'] = MoveGroupCommander('LeftLeg', description)
self.groups['upper_body'] = MoveGroupCommander('UpperBody',
description)
self.groups['lower_body'] = MoveGroupCommander('LowerBody',
description)
self.groups['whole_body'] = MoveGroupCommander('WholeBody',
description)
# initialize end-effectors dict
self.end_effectors = {}
# fill both dict
for key, value in self.groups.iteritems():
self.end_effectors[key] = value.get_end_effector_link()
# add the list of end-effectors for bodies
self.end_effectors['upper_body'] = [
self.end_effectors['head'], self.end_effectors['right_arm'],
self.end_effectors['left_arm']
]
self.end_effectors['lower_body'] = [
self.end_effectors['right_leg'], self.end_effectors['left_leg']
]
self.end_effectors['whole_body'] = self.end_effectors[
'upper_body'] + self.end_effectors['lower_body']
self.prefix = prefix
self.urdf_reader = URDFReader()
rospy.wait_for_service('compute_fk')
self.compute_fk = rospy.ServiceProxy('compute_fk', GetPositionFK)
self.current_state = self.get_initial_state()
def find_common_root(self, link1, link2):
return self.urdf_reader.find_common_root(link1, link2)
def get_links_chain(self, from_link, to_link):
return self.urdf_reader.get_links_chain(from_link, to_link)
def get_joints_chain(self, from_link, to_link):
return self.urdf_reader.get_joints_chain(from_link, to_link)
def extract_group_joints(self, group_name, joint_state):
active = self.groups[group_name].get_active_joints()
res = []
for joint in active:
index = joint_state.name.index(joint)
res.append(joint_state.position[index])
return res
def forward_kinematic(self, joint_state, base='base', links=None):
def compute_fk_client():
try:
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = self.prefix + '/base'
rs = RobotState()
rs.joint_state = joint_state
res = self.compute_fk(header, links, rs)
return res.pose_stamped
except rospy.ServiceException, e:
print "Service call failed: %s" % e
# in case of troubles return 0 pose stamped
return []
if links is None:
links = self.end_effectors['whole_body']
if type(links) is not list:
if links == "all":
links = self.get_link_names('whole_body')
else:
links = [links]
# check that the base is in links
if base != 'base' and base not in links:
links.append(base)
pose_stamped_list = compute_fk_client()
if not pose_stamped_list:
return {}
# transform it in a dict of poses
pose_dict = {}
if base != 'base':
tr_base = transformations.pose_to_list(
pose_stamped_list[links.index(base)].pose)
inv_base = transformations.inverse_transform(tr_base)
for i in range(len(links)):
if links[i] != base:
tr = transformations.pose_to_list(
pose_stamped_list[i].pose)
pose_dict[links[i]] = transformations.multiply_transform(
inv_base, tr)
else:
for i in range(len(links)):
pose_dict[links[i]] = transformations.pose_to_list(
pose_stamped_list[i].pose)
return pose_dict