def test_get_interpolated_setpoints_invalid_time_too_high(self): setpoint = self.joint_trajectory.get_interpolated_setpoint( self.duration + Duration(seconds=1)) self.assertEqual( setpoint, Setpoint(self.duration + Duration(seconds=1), self.setpoints[-1].position, 0), )
def setUp(self): self.joint_name = "test_joint" self.limits = Limits(-1, 1, 2) self.duration = Duration(seconds=2.0) self.times = [Duration(), self.duration / 2.0, self.duration] self.setpoints = [ Setpoint(t, 2 * t.seconds, t.seconds / 2.0) for t in self.times ] self.joint_trajectory = JointTrajectory(self.joint_name, self.limits, self.setpoints, self.duration)
def _parse_duration_parameter(self, name: str) -> Duration: """Get a duration parameter from the parameter server. Returns 0 if the parameter does not exist. Clamps the duration to 0 if it is negative. """ if self.has_parameter(name): value = self.get_parameter(name).value if value < 0: value = 0 return Duration(seconds=value) else: return Duration(0)
def test_interpolate_trajectories_correct_duration(self): parameter = 0.5 other_duration = self.duration + Duration(seconds=1) other_times = [Duration(), other_duration / 2.0, other_duration] other_setpoints = [ Setpoint(t, 2 * t.seconds, t.seconds / 2.0) for t in other_times ] other_trajectory = JointTrajectory(self.joint_name, self.limits, other_setpoints, other_duration) new_trajectory = JointTrajectory.interpolate_joint_trajectories( self.joint_trajectory, other_trajectory, parameter) self.assertEqual( new_trajectory.duration, self.duration.weighted_average(other_duration, parameter), )
def test_interpolation_correct(self): time, position, velocity = 1, 1, 1 parameter = 0.3 other_setpoint = Setpoint(Duration(seconds=time), position, velocity) expected_result = Setpoint( Duration( seconds=self.setpoint.time.seconds * (1 - parameter) + parameter * time ), self.setpoint.position * (1 - parameter) + parameter * position, self.setpoint.velocity * (1 - parameter) + parameter * velocity, ) self.assertEqual( expected_result, Setpoint.interpolate_setpoints(self.setpoint, other_setpoint, parameter), )
def __init__(self, node: Node, gait_name: str = "balanced_walk", default_walk: Gait = None): self.gait_name = gait_name self._node = node self._default_walk = default_walk self._constructing = False self.logger = Logger(self._node, __class__.__name__) self._current_subgait = None self._current_subgait_duration = Duration(0) self._start_time = None self._end_time = None self._current_time = None self.capture_point_event = Event() self.capture_point_result = None self._capture_point_service = { "left_leg": node.create_client(srv_name="/march/capture_point/foot_left", srv_type=CapturePointPose), "right_leg": node.create_client(srv_name="/march/capture_point/foot_right", srv_type=CapturePointPose), } self.moveit_event = Event() self.moveit_trajectory_result = None self._moveit_trajectory_service = node.create_client( srv_name="/march/moveit/get_trajectory", srv_type=GetMoveItTrajectory)
def start( self, current_time: Time, first_subgait_delay: Optional[Duration] = ZERO_DURATION, ) -> GaitUpdate: """Start the gait. Creates a trajectory command to go towards the idle position in the given duration. :returns Returns a GaitUpdate that usually contains a TrajectoryCommand. """ if first_subgait_delay is not None and first_subgait_delay > Duration( 0): self._start_time = current_time + first_subgait_delay self._end_time = self._start_time + self._duration self._scheduled_early = True return GaitUpdate.should_schedule_early( TrajectoryCommand( self._get_trajectory_msg(), self._duration, self._name, self._start_time, )) else: self._start_time = current_time self._end_time = self._start_time + self._duration return GaitUpdate.should_schedule( TrajectoryCommand( self._get_trajectory_msg(), self._duration, self._name, self._start_time, ))
def schedule(self, command: TrajectoryCommand): """Schedules a new trajectory. :param TrajectoryCommand command: The trajectory command to schedule """ self._failed = False stamp = command.start_time.to_msg() command.trajectory.header.stamp = stamp goal = FollowJointTrajectoryGoal(trajectory=command.trajectory) self._trajectory_goal_pub.publish( FollowJointTrajectoryActionGoal( header=Header(stamp=stamp), goal_id=GoalID(stamp=stamp, id=str(command)), goal=goal, ) ) info_log_message = f"Scheduling {command.name}" debug_log_message = f"Subgait {command.name} starts " if self._node.get_clock().now() < command.start_time: time_difference = Duration.from_ros_duration( command.start_time - self._node.get_clock().now() ) debug_log_message += f"in {round(time_difference.seconds, 3)}s" else: debug_log_message += "now" self._goals.append(command) self.logger.info(info_log_message) self.logger.debug(debug_log_message)
def __init__(self, gait_name, subgaits, graph): super(SemiDynamicSetpointsGait, self).__init__(f"dynamic_{gait_name}", subgaits, graph) self._should_freeze = False self._is_frozen = False self._freeze_duration = Duration(0) self._freeze_position = None
def get_joint_states_from_foot_state(foot_state: Foot, time: float) -> dict: """Translate between feet_state and a list of setpoints. :param foot_state: A fully populated Foot object. :param time: The time of the Foot state and resulting setpoints. :return: A dictionary of setpoints, the foot location and velocity of which corresponds with the feet_state. """ joint_states = Foot.calculate_joint_angles_from_foot_position( foot_state.position, foot_state.foot_side, time) # Find the joint angles a moment later using the foot position a # moment later use this together with the current joint angles to # calculate the joint velocity next_position = Foot.calculate_next_foot_position(foot_state) next_joint_positions = Foot.calculate_joint_angles_from_foot_position( next_position.position, next_position.foot_side, time + Duration(seconds=VELOCITY_SCALE_FACTOR), ) for joint in JOINT_NAMES_IK: if joint in joint_states and joint in next_joint_positions: joint_states[joint].add_joint_velocity_from_next_angle( next_joint_positions[joint]) return joint_states
def start( self, current_time: Time, first_subgait_delay: Optional[ Duration] = DEFAULT_FIRST_SUBGAIT_START_DELAY, ) -> GaitUpdate: """Start the gait. Sets current subgait to the first subgait, resets the time and generates the first trajectory command. May optionally delay the first subgait. :param first_subgait_delay Optional duration to delay the first subgait by. :return: A TrajectoryCommand message with the trajectory of the first subgait. """ self._reset() self._current_time = current_time self._current_subgait = self.subgaits[self.graph.start_subgaits()[0]] self._next_subgait = self._current_subgait # Delay first subgait if duration is greater than zero if first_subgait_delay > Duration(0): self._start_is_delayed = True self._update_time_stamps(self._current_subgait, first_subgait_delay) return GaitUpdate.should_schedule_early( self._command_from_current_subgait()) else: self._start_is_delayed = False self._update_time_stamps(self._current_subgait) return GaitUpdate.should_schedule( self._command_from_current_subgait())
def test_update_subgait_start_delayed_did_start(self): self.gait.start(Time(seconds=0), Duration(seconds=3)) gait_update = self.gait.update(Time(seconds=3.5)) self.assertEqual(self.gait.subgait_name, "right_open") self.assertEqual(self.gait._start_time, Time(seconds=3)) self.assertEqual(self.gait._end_time, Time(seconds=4.5)) self.assertFalse(self.gait._start_is_delayed) self.assertEqual(gait_update, GaitUpdate.subgait_updated())
def test_update_subgait_schedule_early(self): self.gait.start(Time(seconds=0)) gait_update = self.gait.update(Time(seconds=1), Duration(seconds=0.8)) self.assertEqual(self.gait.subgait_name, "right_open") self.assertEqual(self.gait._start_time, Time(seconds=0)) self.assertEqual(self.gait._end_time, Time(seconds=1.5)) self.assertTrue(self.gait._scheduled_early) self.assertEqual( gait_update, GaitUpdate.should_schedule_early( TrajectoryCommand.from_subgait( self.gait.subgaits["left_swing"], Time(seconds=1.5))), ) gait_update = self.gait.update(Time(seconds=1.6), Duration(seconds=0.8)) self.assertEqual(gait_update, GaitUpdate.subgait_updated())
def from_dict( cls, robot: urdf.Robot, subgait_dict: dict, gait_name: str, subgait_name: str, version: str, ): """ List parameters from the yaml file in organized lists. :param robot: The robot corresponding to the given sub-gait file :param subgait_dict: The dictionary extracted from the yaml file :param gait_name: The name of the parent gait :param subgait_name: The name of the child (sub)gait :param version: The version of the yaml file :returns A populated Subgait object """ if robot is None: raise GaitError("Cannot create gait without a loaded robot.") duration = Duration(nanoseconds=subgait_dict["duration"]) joint_list = [] for name, points in sorted(subgait_dict["joints"].items(), key=lambda item: item[0]): urdf_joint = cls.joint_class.get_joint_from_urdf(robot, name) if urdf_joint is None or urdf_joint.type == "fixed": continue limits = Limits.from_urdf_joint(urdf_joint) joint_list.append( cls.joint_class.from_setpoint_dict(name, limits, points, duration)) subgait_type = (subgait_dict["gait_type"] if subgait_dict.get("gait_type") else "") subgait_description = (subgait_dict["description"] if subgait_dict.get("description") else "") return cls( joint_list, duration, subgait_type, gait_name, subgait_name, version, subgait_description, robot, )
def from_begin_point(self, begin_time: Duration, position: float) -> None: """ Manipulate the gait to start at given time. Removes all set points after the given begin time. Adds the begin position with 0 velocity at the start. It also adds 1 second at the beginning to allow speeding up to the required speed again. :param begin_time: The time to start :param position: The position to start from """ begin_time -= Duration(seconds=1) for setpoint in reversed(self.setpoints): if setpoint.time <= begin_time: self.setpoints.remove(setpoint) else: setpoint.time -= begin_time self.setpoints = [ Setpoint(time=Duration(), position=position, velocity=0) ] + self.setpoints self._duration = self._duration - begin_time
def setUp(self): self.setpoint = Setpoint( Duration(seconds=1.123412541), 0.034341255, 123.162084549 ) # 0.0343412512 123.162084 self.setpoint_dict = { "left_hip_aa": self.setpoint, "left_hip_fe": self.setpoint, "left_knee": self.setpoint, "right_hip_aa": self.setpoint, "right_hip_fe": self.setpoint, "right_knee": self.setpoint, }
def test_valid_boundary_points_nonzero_start_end_zero_speed(self): # First setpoint at t = 0.5 and last setpoint at t = 1.5 =/= duration have zero speed. setpoints = [ Setpoint( t * 0.5 + Duration(seconds=0.5), (self.duration - t).seconds, t.seconds * 2 - t.seconds**2, ) for t in self.times ] joint_trajectory = JointTrajectory(self.joint_name, self.limits, setpoints, self.duration) self.assertTrue(joint_trajectory._validate_boundary_points())
def _get_extra_ankle_setpoint(self) -> Setpoint: """Returns an extra setpoint for the swing leg ankle that can be used to create a push off. :returns: An extra setpoint for the swing leg ankle :rtype: Setpoint """ return Setpoint( Duration(self.time[SetpointTime.PUSH_OFF_INDEX]), self.push_off_position, 0.0, )
def test_invalid_boundary_points_nonzero_start_nonzero_speed(self): # First setpoint at t = 1 has nonzero speed. setpoints = [ Setpoint( t * 0.5 + Duration(seconds=1), (self.duration - t).seconds, (self.duration - t).seconds * 2, ) for t in self.times ] joint_trajectory = JointTrajectory(self.joint_name, self.limits, setpoints, self.duration) self.assertFalse(joint_trajectory._validate_boundary_points())
def test_weighted_average_states(self): parameter = 0.8 base_right_foot = Foot(Side.right, Vector3d(0, 0, 0), Vector3d(0, 0, 0)) base_left_foot = Foot(Side.left, Vector3d(0, 0, 0), Vector3d(0, 0, 0)) base_state = FeetState(base_right_foot, base_left_foot, Duration(seconds=0.1)) other_right_foot = Foot(Side.right, Vector3d(1, 1, 1), Vector3d(1, 1, 1)) other_left_foot = Foot(Side.left, Vector3d(1, 1, 1), Vector3d(1, 1, 1)) other_state = FeetState( other_right_foot, other_left_foot, Duration(seconds=0.1) ) resulting_state = FeetState.weighted_average_states( base_state, other_state, parameter ) self.assertEqual(Vector3d(0.8, 0.8, 0.8), resulting_state.left_foot.position) self.assertEqual(Vector3d(0.8, 0.8, 0.8), resulting_state.left_foot.velocity) self.assertEqual(Vector3d(0.8, 0.8, 0.8), resulting_state.right_foot.position) self.assertEqual(Vector3d(0.8, 0.8, 0.8), resulting_state.right_foot.velocity)
def test_find_known_position_forward(self): setpoint_dict = { "left_ankle": Setpoint(Duration(), 0, 0), "left_hip_aa": Setpoint(Duration(), 0, 0), "left_hip_fe": Setpoint(Duration(), pi / 2, 0), "left_knee": Setpoint(Duration(), 0, 0), "right_ankle": Setpoint(Duration(), 0, 0), "right_hip_aa": Setpoint(Duration(), 0, 0), "right_hip_fe": Setpoint(Duration(), pi / 2, 0), "right_knee": Setpoint(Duration(), 0, 0), } [ l_ul, l_ll, l_hl, l_ph, r_ul, r_ll, r_hl, r_ph, base, ] = get_lengths_robot_for_inverse_kinematics(Side.both) resulting_state = FeetState.from_setpoint_dict(setpoint_dict) expected_right_foot = Foot( Side.right, Vector3d(r_ul + r_ll + r_hl, base / 2.0 + r_ph, 0), Vector3d(0, 0, 0), ) expected_left_foot = Foot( Side.right, Vector3d(l_ul + l_ll + l_hl, -base / 2.0 - l_ph, 0), Vector3d(0, 0, 0), ) expected_state = FeetState(expected_right_foot, expected_left_foot, Duration()) self.assertTrue( Vector3d.is_close_enough( expected_state.left_foot.position, resulting_state.left_foot.position ) ) self.assertTrue( Vector3d.is_close_enough( expected_state.right_foot.position, resulting_state.right_foot.position ) )
def _joint_dict_to_setpoint_dict(joint_dict: dict) -> dict: """Creates a setpoint_dict from a joint_dict. :param joint_dict: A dictionary containing joint names and positions. :type: dict :returns: A dictionary containing joint names and setpoints. :rtype: dict """ setpoint_dict = {} for name, position in joint_dict.items(): setpoint_dict[name] = Setpoint(Duration(0), position, 0) return setpoint_dict
def test_start_delayed(self): gait_update = self.gait.start(Time(seconds=0), Duration(seconds=1)) self.assertEqual(self.gait.subgait_name, "right_open") self.assertEqual(self.gait._start_time, Time(seconds=1)) self.assertEqual(self.gait._end_time, Time(seconds=2.5)) self.assertTrue(self.gait._start_is_delayed) self.assertEqual( gait_update, GaitUpdate.should_schedule_early( self.gait._command_from_current_subgait()), )
def test_inverse_kinematics_reversed_position(self): right_foot = Foot(Side.right, Vector3d(0.18, 0.08, 0.6), Vector3d(0, 0, 0)) left_foot = Foot(Side.left, Vector3d(0.18, -0.08, 0.6), Vector3d(0, 0, 0)) desired_state = FeetState(right_foot, left_foot, Duration(seconds=0.1)) new_setpoints = FeetState.feet_state_to_setpoints(desired_state) resulting_position = FeetState.from_setpoint_dict(new_setpoints) dif_left = ( desired_state.left_foot.position - resulting_position.left_foot.position ) dif_right = ( desired_state.right_foot.position - resulting_position.right_foot.position ) self.assertLess(dif_left.norm(), 0.0001) self.assertLess(dif_right.norm(), 1 / 0.0001)
def _current_gait_cb(self, gait_name, subgait_name, version, duration: Duration, gait_type): """Standard callback when gait changes, publishes the current gait More callbacke can be added using add_gait_callback""" self.logger.debug(f"Current subgait updated to {subgait_name}") self.current_gait_pub.publish( CurrentGait( header=Header( stamp=self._gait_selection.get_clock().now().to_msg()), gait=gait_name, subgait=subgait_name, version=version, duration=duration.to_msg(), gait_type=gait_type, ))
def _new_trajectory_command(self) -> TrajectoryCommand: """Update the trajectory values and generate a new trajectory command. :return Return a TrajectoryCommand for the next subgait. """ trajectory = self.get_joint_trajectory_msg(self._current_subgait) time_from_start = trajectory.points[-1].time_from_start self._current_subgait_duration = Duration.from_msg(time_from_start) self._start_time = self._current_time self._end_time = self._start_time + self._current_subgait_duration return TrajectoryCommand( trajectory, self._current_subgait_duration, self.subgait_name, self._start_time, )
def get_interpolated_setpoint(self, time: Duration) -> Setpoint: """Get a setpoint on a certain time. If there is no setpoint at this time point, it will interpolate between the setpoints. """ if time < Duration(seconds=0): return self.setpoint_class(time, self.setpoints[0].position, 0) if time > self.duration: return self.setpoint_class(time, self.setpoints[-1].position, 0) return self.setpoint_class( time, float(self.interpolated_position(time.seconds)), float(self.interpolated_velocity(time.seconds)), )
def from_setpoint_dict(cls, name: str, limits: Limits, setpoint_dict: List[dict], duration: Duration) -> JointTrajectory: """Creates a list of joint trajectories. :param str name: Name of the joint :param limits: Joint limits from the URDF :param list(dict) setpoints: A list of setpoints from the subgait configuration :param duration: The total duration of the trajectory """ setpoints = [ Setpoint( time=Duration(nanoseconds=setpoint["time_from_start"]), position=setpoint["position"], velocity=setpoint["velocity"], ) for setpoint in setpoint_dict ] return cls(name, limits, setpoints, duration)
def calculate_next_positions_joint(cls, setpoint_dic: dict) -> dict: """ Calculate the position of the joints a moment later. Calculates using the approximation: next_position = position + current_velocity * time_difference :param setpoint_dic: A dictionary of setpoints with positions and velocities :return: A dictionary with the positions of the joints 1 / VELOCITY_SCALE_FACTOR seconds later """ next_positions = {} for joint in setpoint_dic.keys(): next_positions[joint] = cls( setpoint_dic[joint].time + Duration(seconds=VELOCITY_SCALE_FACTOR), setpoint_dic[joint].position + setpoint_dic[joint].velocity * VELOCITY_SCALE_FACTOR, ) return next_positions
def _get_trajectory_command(self, stop=False) -> TrajectoryCommand: """Return a TrajectoryCommand based on current subgait_id :return: TrajectoryCommand with the current subgait and start time. :rtype: TrajectoryCommand """ if self._start_is_delayed: self._end_time = self._start_time if stop: self._end = True self.logger.info("Stopping dynamic gait.") else: self.foot_location = self._get_foot_location(self.subgait_id) stop = self._check_msg_time(self.foot_location) self._publish_foot_location(self.subgait_id, self.foot_location) self.logger.info( f"Stepping to location ({self.foot_location.point.x}, {self.foot_location.point.y})" ) self.dynamic_subgait = DynamicSubgait( self.gait_selection, self.start_position, self.subgait_id, self.joint_names, self.foot_location.point, self.joint_soft_limits, stop, ) trajectory = self.dynamic_subgait.get_joint_trajectory_msg() return TrajectoryCommand( trajectory, Duration(self.dynamic_subgait_duration), self.subgait_id, self._end_time, )