def setUpClass(cls):
rospy.wait_for_message('/move_group/status', GoalStatusArray)
cls.hand_type = rospy.get_param('~test_hand_and_arm_sim/hand_type')
cls.scene = rospy.get_param('~test_hand_and_arm_sim/scene')
# ur-specific launch files do not accept 'side' param as it is already set
# for phantom hands use hand finder
try:
cls.side = rospy.get_param('~test_hand_and_arm_sim/side')
if cls.side == 'right':
cls.hand_id = 'rh'
elif cls.side == 'left':
cls.hand_id = 'lh'
except rospy.ROSException:
rospy.loginfo("No side param for this test type")
cls.hand_id = rospy.get_param('/hand/mapping/0')
if cls.hand_id == 'rh':
cls.arm_id = 'ra'
cls.robot_commander = SrRobotCommander(name="right_arm_and_hand")
cls.hand_commander = SrHandCommander(name='right_hand')
cls.arm_commander = SrArmCommander(name='right_arm',
set_ground=not (cls.scene))
elif cls.hand_id == 'lh':
cls.arm_id = 'la'
cls.robot_commander = SrRobotCommander(name="left_arm_and_hand")
cls.hand_commander = SrHandCommander(name='left_hand')
cls.arm_commander = SrArmCommander(name='left_arm',
set_ground=not (cls.scene))
rospy.Subscriber('/move_group/monitored_planning_scene', PlanningScene,
cls.scene_data_cb)
rospy.sleep(10)
def setUpClass(cls):
rospy.wait_for_message("/move_group/status", GoalStatusArray, 60.0)
rospy.wait_for_service("/gazebo/set_model_configuration", 60.0)
rospy.wait_for_message("/clock", Clock, 60.0)
rospy.sleep(10.0) # Wait for Gazebo to sort itself out
cls.robot_commander = SrRobotCommander("right_arm")
cls.robot_commander.set_planner_id("BiTRRT")
cls.eef = cls.robot_commander.get_end_effector_link()
cls.add_ground_plane()
def setUpClass(cls):
rospy.wait_for_message('/move_group/status', GoalStatusArray)
cls.scene = rospy.get_param('~test_hand_and_arm_sim/scene')
cls.robot_commander = SrRobotCommander(name="two_arms_and_hands")
cls.hand_commander = SrHandCommander(name='two_hands')
cls.arm_commander = SrArmCommander(name='two_arms',
set_ground=not (cls.scene))
rospy.Subscriber('/move_group/monitored_planning_scene', PlanningScene,
cls.scene_data_cb)
rospy.sleep(10)
def setUpClass(cls):
rospy.wait_for_message('/move_group/status', GoalStatusArray)
cls.hand_type = 'hand_e'
cls.robot_commander = SrRobotCommander(name="two_arms_and_hands")
cls.rh_commander = SrHandCommander(name='right_hand')
cls.lh_commander = SrHandCommander(name='left_hand')
cls.hand_commander = SrHandCommander(name='two_hands')
cls.arm_commander = SrArmCommander(name='two_arms', set_ground=False)
cls.arm_commander.set_max_velocity_scaling_factor(0.2)
cls.robot_commander.set_max_velocity_scaling_factor(0.2)
rospy.sleep(3)
def __init__(self,
name,
hand_or_arm="both",
side="right",
save_target=False):
self._save = rospy.ServiceProxy('save_robot_state', SaveState)
self._name = name
self._save_target = save_target
if name is None or name == '':
raise ValueError("Cannot save with empty name.")
prefix = ""
double_error = []
if hand_or_arm == "arm":
if side != "bimanual":
prefix = "ra" if side == "right" else "la"
self._commander = SrArmCommander(side + '_arm')
if not self._commander.arm_found():
double_error.append("Group " + side + "_arm not found.")
else:
self._commander = SrRobotCommander('two_arms')
elif hand_or_arm == 'hand':
if side != "bimanual":
prefix = "rh" if side == "right" else "lh"
self._commander = SrHandCommander(side + '_hand')
else:
self._commander = SrRobotCommander('two_hands')
elif hand_or_arm == 'both':
if side != "bimanual":
self._commander = SrRobotCommander(side + '_arm_and_hand')
else:
self._commander = SrRobotCommander('two_arms_and_hands')
if len(double_error) != 0:
raise ValueError(" ".join(double_error))
self._save_hand = (hand_or_arm == "hand" or hand_or_arm == "both")
self._save_arm = (hand_or_arm == "arm" or hand_or_arm == "both")
self._save_bimanual = (side == 'bimanual')
if save_target:
rospy.loginfo("Saving targets instead of current values")
self._mutex = Lock()
self._target_values = dict()
if self._save_hand:
self._hand_subscriber = rospy.Subscriber(
"/" + prefix + "_trajectory_controller/state",
JointTrajectoryControllerState, self._target_cb)
if self._save_arm:
self._arm_subscriber = rospy.Subscriber(
"/" + prefix + "_trajectory_controller/state",
JointTrajectoryControllerState, self._target_cb)
if self._save_bimanual:
if self._save_hand:
self.r_hand_subscriber = rospy.Subscriber(
"/ra_trajectory_controller/state",
JointTrajectoryControllerState, self._target_cb)
self.l_hand_subscriber = rospy.Subscriber(
"/la_trajectory_controller/state",
JointTrajectoryControllerState, self._target_cb)
if self._save_arm:
self.r_arm_subscriber = rospy.Subscriber(
"/ra_trajectory_controller/state",
JointTrajectoryControllerState, self._target_cb)
self.l_arm_subscriber = rospy.Subscriber(
"/la_trajectory_controller/state",
JointTrajectoryControllerState, self._target_cb)
current_dict = {}
rospy.loginfo("Getting current")
current_dict = self._commander.get_current_state()
robot_name = self._commander.get_robot_name()
if self._save_target:
rospy.loginfo("Getting targets")
waiting_for_targets = True
while waiting_for_targets and not rospy.is_shutdown():
self._mutex.acquire()
waiting_for_targets = False
for joint in current_dict:
if joint in self._target_values:
current_dict[joint] = self._target_values[joint]
else:
waiting_for_targets = True
rospy.loginfo("Still waiting for %s target" % joint)
self._mutex.release()
if waiting_for_targets:
rospy.loginfo(self._target_values)
rospy.sleep(1)
if rospy.is_shutdown():
exit(0)
self.save_state(current_dict, robot_name)
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from sr_robot_commander.sr_robot_commander import SrRobotCommander
from sr_robot_commander.sr_arm_commander import SrArmCommander
from sr_robot_commander.sr_hand_commander import SrHandCommander
import geometry_msgs.msg
from builtins import input
rospy.init_node("hand_arm_waypoints", anonymous=True)
# The constructors for the SrArmCommander, SrHandCommander and SrRobotCommander
# take a name parameter that should match the group name of the robot to be used.
# How to command the hand or arm separately
hand_commander = SrHandCommander(name="right_hand")
arm_commander = SrArmCommander(name="right_arm")
# How to command the arm and hand together
robot_commander = SrRobotCommander(name="right_arm_and_hand")
arm_commander.set_max_velocity_scaling_factor(0.1)
rospy.sleep(1.0)
arm_home_joints_goal = {
'ra_shoulder_pan_joint': 0.00,
'ra_elbow_joint': 2.00,
'ra_shoulder_lift_joint': -1.25,
'ra_wrist_1_joint': -0.733,
'ra_wrist_2_joint': 1.5708,
'ra_wrist_3_joint': 0.00
}
arm_hand_home_joints_goal = {
'ra_shoulder_pan_joint': 0.00,
# external_control_loop:=true sim:=false scene:=true
# simulated robot:
# roslaunch sr_robot_launch sr_right_ur10arm_hand.launch sim:=true scene:=true
#
# It is recommended to run this script in simulation first.
from __future__ import absolute_import
import rospy
import sys
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from sr_robot_commander.sr_robot_commander import SrRobotCommander
from builtins import input
rospy.init_node("right_hand_manipulator_plan_quality", anonymous=True)
robot_commander = SrRobotCommander(name="right_arm_and_manipulator")
robot_commander.set_max_velocity_scaling_factor(0.4)
rospy.sleep(2.0)
arm_manipulator_home_joints_goal = {'ra_shoulder_pan_joint': 0.00, 'ra_elbow_joint': 2.00,
'ra_shoulder_lift_joint': -1.25, 'ra_wrist_1_joint': -0.733,
'ra_wrist_2_joint': 1.5708, 'ra_wrist_3_joint': 0.00,
'rh_WRJ1': 0.0, 'rh_WRJ2': 0.0}
example_goal_1 = [1.18773740212, 0.175152574887,
1.00722873872, -0.509149713327,
0.504302807033, -0.490693581737,
0.49564610064]
example_goal_2 = [1.03497881882, 0.174190990124,
from sr_robot_commander.sr_arm_commander import SrArmCommander
from sr_robot_commander.sr_hand_commander import SrHandCommander
from sr_robot_commander.sr_robot_commander import SrRobotCommander
if dropdown_commander == 'arm_commander':
arm_commander = SrArmCommander(set_ground=False)
elif dropdown_commander == 'hand_commander':
hand_commander = SrHandCommander(name=side + "_hand")
elif dropdown_commander == 'robot_commander':
robot_commander = SrRobotCommander(name=side + "_arm_and_hand")
elif dropdown_commander == 'hand_h_and_arm':
robot_commander = SrRobotCommander(name="right_arm_and_hand", prefix="H1_")
else:
arm_commander = SrArmCommander(set_ground=False)
hand_commander = SrHandCommander(name=side + "_hand")
default='radians',
help="ANGLE_TYPE should be either degrees or radians")
args = parser.parse_args()
angle_type = args.angle_type
if angle_type not in ['radians', 'degrees']:
parser.print_help()
exit(1)
scale = 1
if angle_type == "degrees":
scale = 1 * (180 / pi)
# The constructor for SrRobotCommander
# take a name parameter that should match the group name of the robot to be used.
robot_commander = SrRobotCommander(name="right_arm_and_hand")
# get_joints_position returns a dictionary with joints poisitions
all_joints_state = robot_commander.get_joints_position()
hand_joints_state = {
k: (v * scale)
for k, v in all_joints_state.items()
if k.startswith("rh_") and not k.startswith("rh_W")
}
arm_joints_state = {
k: (v * scale)
for k, v in all_joints_state.items()
if k.startswith("ra_") or k.startswith("rh_W")
}