Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Pandas #92

Open
wants to merge 7 commits into
base: master
Choose a base branch
from
Open

Pandas #92

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
62aef21
basic cartesian motion for each panda arm
Jul 23, 2021
55e26c8
added gripper interface, error clearing
Nov 5, 2021
287fdfd
fixed comments from pr
acsipos Mar 22, 2022
791e280
overwrote follow_arms_joint_trajectory with move_group execute
acsipos Mar 22, 2022
f611b29
updated gripper commands and fixed planning groups
acsipos Mar 31, 2022
56cb555
added fake execution flag
acsipos Apr 1, 2022
ec1ec8a
push andrea's stack to pandas branch
acsipos Dec 4, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 4 additions & 24 deletions arm_robots/scripts/tutorial/panda_basic_motion.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,27 +7,7 @@
rospy.init_node('panda_motion')
panda = Panda()
panda.connect()
panda.set_execute(False) # this will stop the robot from actually executing a path, good for testing
# panda.plan_to_position_cartesian(panda.nebula_arm, panda.nebula_wrist, target_position=[0.04, 0.2867, 1.17206])
# panda.plan_to_position_cartesian(panda.rocket_arm, panda.rocket_wrist, target_position=[-0.013, -0.04, 1.16967])
# poking_approach_pose_1 = [0.0, 0.0, 1.07, np.radians(135), np.radians(-90), np.radians(225)]
# poking_target_pose_1 = [-0.039, 0.0, 1.07, np.radians(135), np.radians(-90), np.radians(225)]
# poking_approach_pose_2 = [0.0, 0.015, 1.085, np.radians(135), np.radians(-90), np.radians(225)]
# poking_target_pose_2 = [-0.039, 0.015, 1.085, np.radians(135), np.radians(-90), np.radians(225)]
# poking_approach_pose_3 = [0.0, 0.015, 1.055, np.radians(135), np.radians(-90), np.radians(225)]
# poking_target_pose_3 = [-0.039, 0.015, 1.055, np.radians(135), np.radians(-90), np.radians(225)]
# poking_approach_pose_4 = [0.0, -0.015, 1.055, np.radians(135), np.radians(-90), np.radians(225)]
# poking_target_pose_4 = [-0.039, -0.015, 1.055, np.radians(135), np.radians(-90), np.radians(225)]
# poking_approach_pose_5 = [0.0, -0.015, 1.085, np.radians(135), np.radians(-90), np.radians(225)]
# poking_target_pose_5 = [-0.039, -0.015, 1.085, np.radians(135), np.radians(-90), np.radians(225)]
# pdb.set_trace()
# poking_approach_pose_1 = [0.29320, 0.00387, 0.75, 1.0, 0.0, 0.0, 0.0]
# poking_target_pose_1 = [0.29320, 0.00387, 0.723] #, 1.0, 0.0, 0.0, 0.0]
# time.sleep(1)
# panda.plan_to_pose(panda.rocket_arm, "rocket_EE", poking_approach_pose_1, "world")
# time.sleep(3)
# panda.plan_to_position_cartesian(panda.rocket_arm, "rocket_EE", poking_target_pose_1, 0.001)
# time.sleep(3)
# panda.plan_to_pose(panda.rocket_arm, "rocket_EE", poking_approach_pose_1, "world")
# time.sleep(3)
# TODO: Reference med_motion.py for examples.
# panda.set_execute(False) # this will stop the robot from actually executing a path, good for testing
joint_config = [-0.007018571913499291, -1.3080290837538866, -0.0070561850128466625, -2.598759190173999, -0.008907794188422758, 1.295876371807555, 1.25]

panda.plan_to_joint_config('panda_2', joint_config)
88 changes: 35 additions & 53 deletions arm_robots/src/arm_robots/panda.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,11 @@
#! /usr/bin/env python
PeterMitrano marked this conversation as resolved.
Show resolved Hide resolved
from typing import Optional, Callable, List, Tuple

import actionlib
import rospy
from moveit_msgs.msg import RobotTrajectory
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

from arm_robots.robot import MoveitEnabledRobot
from franka_gripper.msg import GraspAction, GraspGoal, MoveAction, MoveGoal, HomingAction, HomingGoal, StopAction, \
StopGoal
Expand All @@ -11,81 +16,52 @@

class Panda(MoveitEnabledRobot):

def __init__(self, robot_namespace: str = 'combined_panda', force_trigger: float = -0.0, **kwargs):
def __init__(self, robot_namespace: str = '', force_trigger: float = -0.0, **kwargs):
MoveitEnabledRobot.__init__(self,
robot_namespace=robot_namespace,
robot_description=ns_join(robot_namespace, 'robot_description'),
arms_controller_name='position_joint_trajectory_controller',
arms_controller_name=None,
force_trigger=force_trigger,
**kwargs)
self.nebula_arm = "nebula_arm"
self.nebula_id = "panda_1"
self.rocket_arm = "rocket_arm"
self.rocket_id = "panda_2"
self.nebula_wrist = "panda_1_link8"
self.rocket_wrist = "panda_2_link8"
self.nebula_gripper = PandaGripper(self.robot_namespace, self.nebula_id)
self.rocket_gripper = PandaGripper(self.robot_namespace, self.rocket_id)
rospy.Subscriber(ns_join(self.robot_namespace, f'{self.nebula_id}_state_controller/franka_states'), FrankaState,
self.nebula_error_cb)
rospy.Subscriber(ns_join(self.robot_namespace, f'{self.rocket_id}_state_controller/franka_states'), FrankaState,
self.rocket_error_cb)
self.error_client = actionlib.SimpleActionClient(ns_join(self.robot_namespace, "error_recovery"),
ErrorRecoveryAction)
self.error_client.wait_for_server()

def open_nebula_gripper(self):
self.nebula_gripper.move(self.nebula_gripper.MAX_WIDTH)

def close_nebula_gripper(self):
self.nebula_gripper.move(self.nebula_gripper.MIN_WIDTH)

def open_rocket_gripper(self):
self.rocket_gripper.move(self.rocket_gripper.MAX_WIDTH)

def close_rocket_gripper(self):
self.rocket_gripper.move(self.rocket_gripper.MIN_WIDTH)

def nebula_error_cb(self, data):
if data.current_errors.communication_constraints_violation or data.last_motion_errors.communication_constraints_violation:
self.clear_errors(wait_for_result=True)

def rocket_error_cb(self, data):
if data.current_errors.communication_constraints_violation or data.last_motion_errors.communication_constraints_violation:
self.clear_errors(wait_for_result=True)

def clear_errors(self, wait_for_result=False):
goal = ErrorRecoveryGoal()
self.error_client.send_goal(goal)
if wait_for_result:
result = self.error_client.wait_for_result(rospy.Duration(10))
return result
return True

self.panda_1 = 'panda_1'
self.panda_2 = 'panda_2'
self.display_goals = False
# self.panda_1.gripper = PandaGripper(self.robot_namespace, self.panda_1)
# self.panda_2.gripper = PandaGripper(self.robot_namespace, self.panda_2)

def follow_arms_joint_trajectory(self, trajectory: JointTrajectory, stop_condition: Optional[Callable] = None,
group_name: Optional[str] = None):
move_group = self.get_move_group_commander(group_name)
plan_msg = RobotTrajectory()
plan_msg.joint_trajectory = trajectory
move_group.execute(plan_msg)
pass
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

remove this pass


def send_joint_command(self, joint_names: List[str], trajectory_point: JointTrajectoryPoint) -> Tuple[bool, str]:
pass
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this should probably raise NotImplementedError to avoid people accidentally calling this somehow

# TODO: Add control mode setter/getter.


class PandaGripper:
def __init__(self, robot_ns, arm_id):
self.gripper_ns = "/" + robot_ns + "/" + arm_id + "/franka_gripper/"
self.grasp_client = actionlib.SimpleActionClient(self.gripper_ns + "grasp", GraspAction)
self.move_client = actionlib.SimpleActionClient(self.gripper_ns + "move", MoveAction)
self.homing_client = actionlib.SimpleActionClient(self.gripper_ns + "homing", HomingAction)
self.stop_client = actionlib.SimpleActionClient(self.gripper_ns + "stop", StopAction)
self.gripper_ns = ns_join(robot_ns, f'{arm_id}/franka_gripper')
self.grasp_client = actionlib.SimpleActionClient(ns_join(self.gripper_ns, 'grasp'), GraspAction)
self.move_client = actionlib.SimpleActionClient(ns_join(self.gripper_ns, 'move'), MoveAction)
self.homing_client = actionlib.SimpleActionClient(ns_join(self.gripper_ns, 'homing'), HomingAction)
self.stop_client = actionlib.SimpleActionClient(ns_join(self.gripper_ns, 'stop'), StopAction)
self.grasp_client.wait_for_server()
self.move_client.wait_for_server()
self.homing_client.wait_for_server()
self.stop_client.wait_for_server()
self.gripper_width = None
rospy.Subscriber(self.gripper_ns + "joint_states", JointState, self.gripper_cb)
rospy.Subscriber(ns_join(self.gripper_ns, 'joint_states'), JointState, self.gripper_cb)
self.MIN_FORCE = 0.05
self.MAX_FORCE = 50 # documentation says up to 70N is possible as continuous force
self.MIN_WIDTH = 0.0
self.MAX_WIDTH = 0.08
self.DEFAULT_EPSILON = 0.005
self.DEFAULT_SPEED = 0.02
self.DEFAULT_FORCE = 10
# TODO: add to cfg file

def gripper_cb(self, data):
self.gripper_width = data.position[0] + data.position[1]
Expand Down Expand Up @@ -130,3 +106,9 @@ def stop(self, wait_for_result=False):
result = self.stop_client.wait_for_result(rospy.Duration(10))
return result
return True

def open(self):
self.move(self.MAX_WIDTH)

def close(self):
self.move(self.MIN_WIDTH)
28 changes: 15 additions & 13 deletions arm_robots/src/arm_robots/robot.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -105,14 +105,16 @@ def connect(self, preload_move_groups=True):
self.get_move_group_commander(group_name)

def setup_joint_trajectory_controller_client(self, controller_name):
action_name = ns_join(self.robot_namespace, ns_join(controller_name, "follow_joint_trajectory"))
client = SimpleActionClient(action_name, FollowJointTrajectoryAction)
resolved_action_name = rospy.resolve_name(action_name)
wait_msg = f"Waiting for joint trajectory follower server {resolved_action_name}..."
rospy.loginfo(wait_msg)
client.wait_for_server()
rospy.loginfo(f"Connected.")
return client
if controller_name is not None:
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

leave this function alone and override connect to just have:

    def connect(self, preload_move_groups=True):
        self.jacobian_follower.connect_to_psm()
        if preload_move_groups:
            for group_name in self.robot_commander.get_group_names():
                self.get_move_group_commander(group_name)

action_name = ns_join(self.robot_namespace, ns_join(controller_name, "follow_joint_trajectory"))
client = SimpleActionClient(action_name, FollowJointTrajectoryAction)
resolved_action_name = rospy.resolve_name(action_name)
wait_msg = f"Waiting for joint trajectory follower server {resolved_action_name}..."
rospy.loginfo(wait_msg)
client.wait_for_server()
rospy.loginfo(f"Connected.")
return client
return None

def set_execute(self, execute: bool):
self.execute = execute
Expand Down Expand Up @@ -148,7 +150,7 @@ def plan_to_position(self,
if self.raise_on_failure and not planning_result.success:
raise RobotPlanningError(f"Plan to position failed {planning_result.planning_error_code}")

execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition)
execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition, group_name)
return PlanningAndExecutionResult(planning_result, execution_result)

def plan_to_position_cartesian(self,
Expand Down Expand Up @@ -176,7 +178,7 @@ def plan_to_position_cartesian(self,
if self.raise_on_failure and not planning_result.success:
raise RobotPlanningError(f"Cartesian path is only {fraction * 100}% complete")

execution_result = self.follow_arms_joint_trajectory(plan.joint_trajectory, stop_condition)
execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition, group_name)
return PlanningAndExecutionResult(planning_result, execution_result)

def plan_to_pose(self, group_name, ee_link_name, target_pose, frame_id: str = 'robot_root',
Expand All @@ -197,7 +199,7 @@ def plan_to_pose(self, group_name, ee_link_name, target_pose, frame_id: str = 'r
if self.raise_on_failure and not planning_result.success:
raise RobotPlanningError(f"Plan to pose failed {planning_result.planning_error_code}")

execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition)
execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition, group_name)
return PlanningAndExecutionResult(planning_result, execution_result)

def get_link_pose(self, link_name: str):
Expand Down Expand Up @@ -243,7 +245,7 @@ def plan_to_joint_config(self, group_name: str, joint_config: Union[List, Dict,
planning_result = PlanningResult(move_group.plan())
if self.raise_on_failure and not planning_result.success:
raise RobotPlanningError(f"Plan to position failed {planning_result.planning_error_code}")
execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition)
execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition, group_name)
return PlanningAndExecutionResult(planning_result, execution_result)

def make_follow_joint_trajectory_goal(self, trajectory) -> FollowJointTrajectoryGoal:
Expand Down Expand Up @@ -297,7 +299,7 @@ def follow_joint_config(self, joint_names: List[str], joint_positions, client: S
trajectory.points.append(point)
return self.follow_joint_trajectory(trajectory, client)

def follow_arms_joint_trajectory(self, trajectory: JointTrajectory, stop_condition: Optional[Callable] = None):
def follow_arms_joint_trajectory(self, trajectory: JointTrajectory, stop_condition: Optional[Callable] = None, group_name: Optional[str] = None):
PeterMitrano marked this conversation as resolved.
Show resolved Hide resolved
return self.follow_joint_trajectory(trajectory, self.arms_client, stop_condition=stop_condition)

def distance(self, ee_link_name: str, target_position):
Expand Down