def _get_fk_pykdl(self, frame_id, state=None): if state is None: state = self.get_current_state() fk = self._kinematics_pykdl.forward_position_kinematics( dict(zip(state.joint_state.name, state.joint_state.position))) ps = list_to_pose([fk[:3], fk[-4:]], self._world) return self._tf_listener.transformPose(frame_id, ps)
def compute_sub_ik(self, group, desired_dict, result, tol=0.001): # get the desired pose in the correct base frame index = self.links.index(group) base = self.bases[index] tr = desired_dict[group] if base != self.prefix + '/base': if base in desired_dict.keys(): tr_base = desired_dict[base] inv_base = transformations.inverse_transform(tr_base) desired_pose = transformations.multiply_transform(inv_base, tr) else: return 1 else: desired_pose = tr # transform it back to PoseStamped desired_pose = transformations.list_to_pose(desired_pose) try: # call the srv res = self.div_ik_srv[group](desired_poses=[desired_pose], tolerance=tol) joints = res.joint_state.position except: return 1 with self.lock: result[group] = joints
def get_ik(self, eef_poses, seeds=(), source=None, params=None): """ Return IK solutions of this arm's end effector according to the method declared in the constructor :param eef_poses: a PoseStamped or a list [[x, y, z], [x, y, z, w]] in world frame or a list of PoseStamped :param seeds: a single seed or a list of seeds of type RobotState for each input pose :param source: 'robot', 'trac', 'kdl'... the IK source for this call (warning: the source might not be instanciated) :param params: dictionary containing optional non-generic IK parameters :return: a list of RobotState for each input pose in input or a single RobotState TODO: accept also a Point (baxter_pykdl's IK accepts orientation=None) Child methods wait for a *list* of pose(s) and a *list* of seed(s) for each pose """ if not isinstance(eef_poses, list) or isinstance(eef_poses[0], list) and not isinstance(eef_poses[0][0], list): eef_poses = [eef_poses] if not seeds: seeds=[] elif not isinstance(seeds, list): seeds = [seeds]*len(eef_poses) input = [] for eef_pose in eef_poses: if isinstance(eef_pose, list): input.append(list_to_pose(eef_pose, self._world)) elif isinstance(eef_pose, PoseStamped): input.append(eef_pose) else: raise TypeError("ArmCommander.get_ik() accepts only a list of Postamped or [[x, y, z], [x, y, z, w]], got {}".format(str(type(eef_pose)))) output = self._kinematics_services['ik'][self._selected_ik if source is None else source]['func'](input, seeds, params) return output if len(eef_poses)>1 else output[0]
def dicttopath(dic): path = Path() path.header.frame_id = dic["frame_id"] for point in dic["points"]: path.poses.append(list_to_pose(point["pose"], frame_id=dic["frame_id"])) path.poses[-1].header.stamp = Time(point["time"]) return path
def _get_fk_robot(self, frame_id=None, state=None): # Keep this half-working FK, still used by generate_cartesian_path (trajectories.py) if state is not None: raise NotImplementedError( "_get_fk_robot has no FK service provided by the robot except for its current endpoint pose" ) ps = list_to_pose(self.endpoint_pose(), self._world) return self._tf_listener.transformPose(frame_id, ps)
def compute_sub_ik(self, group, desired_dict, result, seed, tol=0.1): # get the desired pose in the correct base frame base = self.links[group] tr = desired_dict[group] if desired_dict: base_found = False if base != self.prefix + '/base': if base in desired_dict.keys(): base_found = True tr_base = desired_dict[base] else: # look for the base in already calculated results timeout = 0.25 start = time() while not base_found and not rospy.is_shutdown() and ( time() - start < timeout): with self.lock: for key, value in result.iteritems(): if base in self.model.get_links_chain( self.links[key], key): base_found = True js = JointState() js.name = value['joint_names'] js.position = value['joint_values'] # call the forward kinematic fk = self.model.forward_kinematic( js, links=base) tr_base = fk[base] if base_found: inv_base = transformations.inverse_transform(tr_base) desired_pose = transformations.multiply_transform( inv_base, tr) else: return 1 else: desired_pose = tr else: return 1 # transform it back to PoseStamped desired_pose = transformations.list_to_pose(desired_pose) desired_pose.header.frame_id = base # try: # call the srv res = self.div_ik_srv[group](desired_poses=[desired_pose], tolerance=tol, seed=seed) joint_state = res.joint_state # except Exception, e: # print e # return 1 with self.lock: result[group] = { 'joint_names': joint_state.name, 'joint_values': joint_state.position }
def _object_grasp_pose_to_world(self, poselist, object): """ Returns the transformation from world to the point defined by "poselist attached to object frame" aka a constraint or a grasp pose. :param poselist: A poselist [[x, y, z], [x, y, z, w]] :param object: The frame where the pose should be attached :return: the transformation from world to the point defined by "poselist attached to object frame" (PoseStamped) """ obj_pose = transformations.list_to_pose(poselist, object) world_pose = self.tfl.transformPose(self.world, obj_pose) return world_pose
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 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 get_ik(self, eef_poses, seeds=(), source=None, params=None): """ Return IK solutions of this arm's end effector according to the method declared in the constructor :param eef_poses: a PoseStamped or a list [[x, y, z], [x, y, z, w]] in world frame or a list of PoseStamped :param seeds: a single seed or a list of seeds of type RobotState for each input pose :param source: 'robot', 'trac', 'kdl'... the IK source for this call (warning: the source might not be instanciated) :param params: dictionary containing optional non-generic IK parameters :return: a list of RobotState for each input pose in input or a single RobotState TODO: accept also a Point (baxter_pykdl's IK accepts orientation=None) Child methods wait for a *list* of pose(s) and a *list* of seed(s) for each pose """ if not isinstance(eef_poses, list) or isinstance( eef_poses[0], list) and not isinstance(eef_poses[0][0], list): eef_poses = [eef_poses] if not seeds: seeds = [] elif not isinstance(seeds, list): seeds = [seeds] * len(eef_poses) input = [] for eef_pose in eef_poses: if isinstance(eef_pose, list): input.append(list_to_pose(eef_pose, self._world)) elif isinstance(eef_pose, PoseStamped): input.append(eef_pose) else: raise TypeError( "ArmCommander.get_ik() accepts only a list of Postamped or [[x, y, z], [x, y, z, w]], got {}" .format(str(type(eef_pose)))) output = self._kinematics_services['ik'][ self._selected_ik if source is None else source]['func'](input, seeds, params) return output if len(eef_poses) > 1 else output[0]
def __init__(self, side): # General attributes self.rospack = rospkg.RosPack() self.side = side self.arm_name = side + '_arm' self.gripper_name = side + '_gripper' # Transform/Geometric attributes self.tfl = tf.TransformListener(True, rospy.Duration( 5 * 60)) # TF Interpolation ON and duration of its cache = 5 minutes self.world = "base" self.scene = rospy.get_param("/thr/scene") with open( self.rospack.get_path("thr_scenes") + "/config/" + self.scene + "/poses.json") as f: self.poses = json.load(f) with open( self.rospack.get_path("thr_action_server") + "/config/action_params.json") as f: self.action_params = json.load(f) with open( self.rospack.get_path("thr_action_server") + "/config/seeds.json") as f: self.seeds = json.load(f) with open( self.rospack.get_path("thr_action_server") + "/config/abilities.json") as f: self.abilities = json.load(f) # Motion/Grasping attributes self.commander = ArmCommander( side, default_kv_max=self.action_params['limits']['kv'], default_ka_max=self.action_params['limits']['ka'], ik='robot', fk='kdl') # Home poses are taken when the server starts: self.starting_state = self.commander.get_current_state() self.tfl.waitForTransform(self.world, self.gripper_name, rospy.Time(0), rospy.Duration(10)) self.starting_pose = transformations.list_to_pose( self.tfl.lookupTransform(self.world, self.gripper_name, rospy.Time(0))) # Checking that the workstation has an acceptable time offset with the robot robot_current_time = self.tfl.getLatestCommonTime( 'base', self.gripper_name).to_sec() local_current_time = time() diff = int(abs(local_current_time - robot_current_time) * 1000) if diff > 250: raise ValueError( "Your workstation's time has an offset of {} ms with the robot," "please synchronize them to prevent using outdated transforms (sudo ntpdate)" .format(diff)) # Action server attributes rospy.loginfo("Starting server " + side) self.server = actionlib.SimpleActionServer( '/thr/robot_run_action/' + side, RunRobotActionAction, self.execute, False) self.result = RunRobotActionActionResult() # Actual actions self.actions = { 'give': Give(self.commander, self.tfl, self.action_params, self.poses, self.seeds, self.server.is_preempt_requested), 'go_home_' + self.side: GoHome(self.commander, self.tfl, self.action_params, self.poses, self.seeds, self.server.is_preempt_requested), 'hold': Hold(self.commander, self.tfl, self.action_params, self.poses, self.seeds, self.server.is_preempt_requested), 'pick': Pick(self.commander, self.tfl, self.action_params, self.poses, self.seeds, self.server.is_preempt_requested), 'grasp': Grasp(self.commander, self.tfl, self.action_params, self.poses, self.seeds, self.server.is_preempt_requested), 'bring_' + self.side: Bring(self.commander, self.tfl, self.action_params, self.poses, self.seeds, self.server.is_preempt_requested), 'place_' + self.side: Place(self.commander, self.tfl, self.action_params, self.poses, self.seeds, self.server.is_preempt_requested), } # On starting blocks... self.server.start() rospy.loginfo(side + ' server ready')
res = compute_ik(poses, fixed_joint_state, tolerance, group_names, seed) return res.joint_state except rospy.ServiceException, e: print "Service call failed: %s" % e # in case of failure return T pose return self.get_initial_state() if seed is None: seed = self.get_current_state() if group_names is not list: group_names = [group_names] # convert the desired poses to PoseStamped poses = [] for key, value in desired_poses.iteritems(): pose = transformations.list_to_pose(value) pose.header.frame_id = key poses.append(pose) # convert the fixed joints to joint state fixed_joint_state = JointState() for key, value in fixed_joints.iteritems(): fixed_joint_state.name += [key] fixed_joint_state.position += [value] return compute_ik_client() def get_joint_values(self, group_name, joint_names=None): if joint_names is None or not joint_names: return self.groups[group_name].get_current_joint_values() else: joint_values = self.groups[group_name].get_current_joint_values() res = []
try: compute_ik = rospy.ServiceProxy('compute_human_ik', GetHumanIK) res = compute_ik(poses, fixed_joint_state, tolerance, group_names, seed) return res.joint_state except rospy.ServiceException, e: print "Service call failed: %s" % e # in case of failure return T pose return self.get_initial_state() if seed is None: seed = self.get_current_state() if group_names is not list: group_names = [group_names] # convert the desired poses to PoseStamped poses = [] for key, value in desired_poses.iteritems(): pose = transformations.list_to_pose(value) pose.header.frame_id = key poses.append(pose) # convert the fixed joints to joint state fixed_joint_state = JointState() for key, value in fixed_joints.iteritems(): fixed_joint_state.name += [key] fixed_joint_state.position += [value] return compute_ik_client() def get_joint_values(self, group_name, joint_names=None): if joint_names is None or not joint_names: return self.groups[group_name].get_current_joint_values() else: joint_values = self.groups[group_name].get_current_joint_values() res = []
# 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 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 if __name__=='__main__': rospy.init_node("test_dmp_traj_discrete") path = Path() for i in range(100): quat = quaternion_about_axis(i*0.00628, (1, 0, 0)) pose = list_to_pose([[i/100., i/100., i/100.], quat]) pose.header.stamp = i/10. path.poses.append(pose) goal = list_to_pose([[-10, -10, -10], [0.707, 0, 0, 0.707]]) rollout = DiscreteTaskSpaceTrajectory(path).rollout(goal) print rollout
def _get_fk_robot(self, frame_id = None, state=None): # Keep this half-working FK, still used by generate_cartesian_path (trajectories.py) if state is not None: raise NotImplementedError("_get_fk_robot has no FK service provided by the robot except for its current endpoint pose") ps = list_to_pose(self.endpoint_pose(), self._world) return self._tf_listener.transformPose(frame_id, ps)
return np.array(y_des).transpose() 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 if __name__ == '__main__': rospy.init_node("test_dmp_traj_discrete") path = Path() for i in range(100): quat = quaternion_about_axis(i * 0.00628, (1, 0, 0)) pose = list_to_pose([[i / 100., i / 100., i / 100.], quat]) pose.header.stamp = i / 10. path.poses.append(pose) goal = list_to_pose([[-10, -10, -10], [0.707, 0, 0, 0.707]]) rollout = DiscreteTaskSpaceTrajectory(path).rollout(goal) print rollout