Binit Shah
2 years ago
2 changed files with 161 additions and 159 deletions
Split View
Diff Options
-
+160 -0hello_helpers/src/hello_helpers/simple_command_group.py
-
+1 -159stretch_core/nodes/command_groups.py
+ 160
- 0
hello_helpers/src/hello_helpers/simple_command_group.py
View File
| @ -0,0 +1,160 @@ | |||
| import hello_helpers.hello_misc as hm | |||
| class SimpleCommandGroup: | |||
| def __init__(self, joint_name, joint_range, acceptable_joint_error=0.015): | |||
| """Simple command group to extend | |||
| Attributes | |||
| ---------- | |||
| name: str | |||
| joint name | |||
| range: tuple(float) | |||
| acceptable joint bounds | |||
| active: bool | |||
| whether joint is active | |||
| index: int | |||
| index of joint's goal in point | |||
| goal: dict | |||
| components of the goal | |||
| error: float | |||
| the error between actual and desired | |||
| acceptable_joint_error: float | |||
| how close to zero the error must reach | |||
| """ | |||
| self.name = joint_name | |||
| self.range = joint_range | |||
| self.active = False | |||
| self.index = None | |||
| self.goal = {"position": None} | |||
| self.error = None | |||
| self.acceptable_joint_error = acceptable_joint_error | |||
| def get_num_valid_commands(self): | |||
| """Returns number of active joints in the group | |||
| Returns | |||
| ------- | |||
| int | |||
| the number of active joints within this group | |||
| """ | |||
| if self.active: | |||
| return 1 | |||
| return 0 | |||
| def update(self, commanded_joint_names, invalid_joints_callback, **kwargs): | |||
| """Activates joints in the group | |||
| Checks commanded joints to activate the command | |||
| group and validates joints used correctly. | |||
| Parameters | |||
| ---------- | |||
| commanded_joint_names: list(str) | |||
| list of commanded joints in the trajectory | |||
| invalid_joints_callback: func | |||
| error callback for misuse of joints in trajectory | |||
| Returns | |||
| ------- | |||
| bool | |||
| False if commanded joints invalid, else True | |||
| """ | |||
| self.active = False | |||
| self.index = None | |||
| if self.name in commanded_joint_names: | |||
| self.index = commanded_joint_names.index(self.name) | |||
| self.active = True | |||
| return True | |||
| def set_goal(self, point, invalid_goal_callback, fail_out_of_range_goal, **kwargs): | |||
| """Sets goal for the joint group | |||
| Sets and validates the goal point for the joints | |||
| in this command group. | |||
| Parameters | |||
| ---------- | |||
| point: trajectory_msgs.JointTrajectoryPoint | |||
| the target point for all joints | |||
| invalid_goal_callback: func | |||
| error callback for invalid goal | |||
| fail_out_of_range_goal: bool | |||
| whether to bound out-of-range goals to range or fail | |||
| Returns | |||
| ------- | |||
| bool | |||
| False if commanded goal invalid, else True | |||
| """ | |||
| self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None} | |||
| if self.active: | |||
| goal_pos = point.positions[self.index] if len(point.positions) > self.index else None | |||
| if goal_pos is None: | |||
| err_str = ("Received goal point with positions array length={0}. " | |||
| "This joint ({1})'s index is {2}. Length of array must cover all joints listed " | |||
| "in commanded_joint_names.").format(len(point.positions), self.name, self.index) | |||
| invalid_goal_callback(err_str) | |||
| return False | |||
| self.goal['position'] = hm.bound_ros_command(self.range, goal_pos, fail_out_of_range_goal) | |||
| self.goal['velocity'] = point.velocities[self.index] if len(point.velocities) > self.index else None | |||
| self.goal['acceleration'] = point.accelerations[self.index] if len(point.accelerations) > self.index else None | |||
| self.goal['contact_threshold'] = point.effort[self.index] if len(point.effort) > self.index else None | |||
| if self.goal['position'] is None: | |||
| err_str = ("Received {0} goal point that is out of bounds. " | |||
| "Range = {1}, but goal point = {2}.").format(self.name, self.range, goal_pos) | |||
| invalid_goal_callback(err_str) | |||
| return False | |||
| return True | |||
| def init_execution(self, robot, robot_status, **kwargs): | |||
| """Starts execution of the point | |||
| Uses Stretch's Python API to begin moving to the | |||
| target point. | |||
| Parameters | |||
| ---------- | |||
| robot: stretch_body.robot.Robot | |||
| top-level interface to Python API | |||
| robot_status: dict | |||
| robot's current status | |||
| """ | |||
| raise NotImplementedError | |||
| def update_execution(self, robot_status, **kwargs): | |||
| """Monitors progress of joint group | |||
| Checks against robot's status to track progress | |||
| towards the target point. | |||
| This method must set self.error. | |||
| Parameters | |||
| ---------- | |||
| robot_status: dict | |||
| robot's current status | |||
| Returns | |||
| ------- | |||
| float/None | |||
| error value if group active, else None | |||
| """ | |||
| raise NotImplementedError | |||
| def goal_reached(self): | |||
| """Returns whether reached target point | |||
| Returns | |||
| ------- | |||
| bool | |||
| if active, whether reached target point, else True | |||
| """ | |||
| if self.active: | |||
| return (abs(self.error) < self.acceptable_joint_error) | |||
| return True | |||