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 setUp(self):
# init
rospy.init_node("mouse_tester", anonymous=True)
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
client.wait_for_server()
rospy.sleep(15.)
self.data = Joy()
self.target_pose = PoseStamped()
start_pose = Pose()
self.new_pose = PoseStamped()
# expected values
self.expvals = [[[0.3, 0.0, 1.1, 0, 0.7, 0.05, 0.05],
[0.51, 0.25, 1.1, 0.7, 0.71, 0.05, 0.048],
[0.51, 0.26, 1.1, 0.7, 0.71, 0.05, 0.048]],
[[0.27, 0.25, 1.1, 0.7, 0.7, 0.05, 0.05],
[0.27, 0.35, 1.1, 0.7, 0.7, 0.05, 0.05],
[0.27, 0.35, 1.1, 0.7, 0.7, 0.05, 0.05]],
[[0.27, 0.25, 1.1, 0.7, 0.7, 0.05, 0.05],
[0.27, 0.25, 0.9, 0.7, 0.7, 0.05, 0.05],
[0.27, 0.25, 0.85, 0.7, 0.7, 0.05, 0.05]],
[[0.27, 0.25, 1.1, 0.7, 0.7, 0.05, 0.05],
[0.27, 0.25, 1.1, 0.69, 0.7, 0.05, 0.1],
[0.27, 0.25, 1, 0.7, 0.7, 0.05, 0.1]],
[[0.27, 0.25, 1.1, 0.7, 0.7, 0.05, 0.05],
[0.27, 0.25, 1.1, 0.69, 0.69, 0.1, 0.1],
[0.27, 0.25, 1.1, 0.69, 0.69, 0.1, 0.1]],
[[0.27, 0.25, 1.1, 0.7, 0.7, 0.05, 0.05],
[0.27, 0.25, 1.1, 0.7, 0.6, 0.05, 0.08],
[0.27, 0.25, 1.1, 0.7, 0.6, 0.05, 0.05]]]
# initialize arm's position
self._arm_commander = SrArmCommander(name="right_arm", set_ground=True)
start_pose = self._arm_commander.get_current_pose()
self.new_pose.header.stamp = rospy.get_rostime()
self.new_pose.header.frame_id = "world"
self.new_pose.pose = start_pose
self.new_pose.pose.position.x = 0.3
self.new_pose.pose.position.y = 0.25
self.new_pose.pose.position.z = 1.1
self.new_pose.pose.orientation.x = 0.7
self.new_pose.pose.orientation.y = 0.7
self.new_pose.pose.orientation.z = 0
self.new_pose.pose.orientation.w = 0
self.sub = rospy.Subscriber("command", PoseStamped, self.comparecall)
self.pub_arm = rospy.Publisher("command", PoseStamped, queue_size=1)
self.pub = rospy.Publisher("/spacenav/joy", Joy, queue_size=1)
# initialise joy publisher
self.data.buttons = [0, 0]
self.data.axes = [0, 0, 0, 0, 0, 0]
self.reset_joy = Joy()
self.reset_joy = self.data
self.pub.publish(self.data)
# move arm to initial position
self._arm_commander.move_to_pose_value_target_unsafe(
self.new_pose, avoid_collisions=True, wait=False)
self.pub_arm.publish(self.new_pose)
# wait for two seconds to finish initialization
rospy.sleep(2)
self.tester()
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)
from moveit_msgs.msg import RobotTrajectory
import sys
import rospy
from sr_robot_commander.sr_hand_commander import SrHandCommander
from sr_utilities.hand_finder import HandFinder
from std_msgs.msg import String
import time
import math
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from sr_robot_commander.sr_arm_commander import SrArmCommander
from sr_robot_commander.sr_arm_commander import *
s = "1_-1"
#rospy.init_node("basic_hand_examples", anonymous=True)
rospy.init_node('listener2', anonymous=True)
arm_commander = SrArmCommander(set_ground=False)
reference_frame = arm_commander.get_pose_reference_frame()
#hand_finder = HandFinder()
#hand_parameters = hand_finder.get_hand_parameters()
#hand_serial = hand_parameters.mapping.keys()[0]
#hand_commander = SrHandCommander(hand_parameters=hand_parameters,hand_serial=hand_serial)
#hand_mapping = hand_parameters.mapping[hand_serial]
#joints = hand_finder.get_hand_joints()[hand_mapping]
print "this", #SrArmCommander.get_end_effector_pose_from_state()
a = 2
print("Setup Done.")
new_position = [0.5, 0.5, 0.5, 0.0, 0.0, 0.0]
plan = arm_commander.plan_to_position_target(new_position)
#!/usr/bin/env python
# Example to demonstrate moving to stored/names targets. Both arm and hand movements executed.
# Available named targets can be viewed in MoveIt, on the planning tab.
import rospy
from sr_robot_commander.sr_arm_commander import SrArmCommander
from sr_robot_commander.sr_hand_commander import SrHandCommander
rospy.init_node("grab_bottle_example", anonymous=True)
hand_commander = SrHandCommander()
arm_commander = SrArmCommander()
print("Executing moves:")
# start a bit higher
# arm_commander.move_to_named_target("gamma", True)
# approach
print("Approach bottle")
hand_commander.move_to_named_target("open_bottle", False)
arm_commander.move_to_named_target("approach_bottle", True)
# grab
print("Hold bottle")
arm_commander.move_to_named_target("hold_bottle", True)
hand_commander.move_to_named_target("hold_bottle", True)
# pour
print("Pour")
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('listener2', anonymous=True)
rospy.Subscriber('chatter', String, callback)
#print "this only loops"
# spin() simply keeps python from exiting until this node is stopped
#rospy.spin()
while __name__ == '__main__':
if a == 1:
global s
s = "1_-1"
#rospy.init_node("basic_hand_examples", anonymous=True)
rospy.init_node('listener2', anonymous=True)
arm_commander = SrArmCommander(set_ground=False)
reference_frame = arm_commander.get_pose_reference_frame()
#hand_finder = HandFinder()
#hand_parameters = hand_finder.get_hand_parameters()
#hand_serial = hand_parameters.mapping.keys()[0]
#hand_commander = SrHandCommander(hand_parameters=hand_parameters,hand_serial=hand_serial)
#hand_mapping = hand_parameters.mapping[hand_serial]
#joints = hand_finder.get_hand_joints()[hand_mapping]
print "this", #SrArmCommander.get_end_effector_pose_from_state()
a = 2
print("Setup Done.")
else:
if a == 2:
listener()
#print "s below", s
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")
#!/usr/bin/env python
# This example demonstrates some of the functions of the arm commander.
# The arm is moved through a sequence of goals generated via different functions in the commander.
import rospy
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from sr_robot_commander.sr_arm_commander import SrArmCommander
rospy.init_node("basic_left_arm_example", anonymous=True)
arm_commander = SrArmCommander(name="left_arm", set_ground=False)
rospy.sleep(rospy.Duration(2))
# The arm commander generates a plan to a new pose before the pose is executed.
pose_1 = [0.5, -0.3, 1.2, 0, 1.57, 0]
print("Planning the move to the first pose:\n" + str(pose_1) + "\n")
arm_commander.plan_to_pose_target(pose_1)
print("Finished planning, moving the arm now.")
# Can only execute if a plan has been generated
arm_commander.execute()
rospy.sleep(6.0)
pose_2 = [0.5, 0.3, 1.2, 0, 1.57, 0]
print("Planning the move to the second pose:\n" + str(pose_2) + "\n")
arm_commander.plan_to_pose_target(pose_2)
print("Finished planning, moving the arm now.")
arm_commander.execute()
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)
self._tdc_db.append(x)
if len(self._tdc_db) >= 50:
self.baseline(name="tdc")
else:
print("Proper name value not provided! ['electrdes', 'pac0', 'pac1', \
'pdc', 'tac', 'tdc']")
return
rospy.init_node("move_test", anonymous=True)
context = zmq.Context()
socket = context.socket(zmq.PUB)
socket.bind("tcp://*:5556")
hand_commander = SrHandCommander()
arm_commander = SrArmCommander()
rate_handle = rospy.Rate(100) #hz
# ff = BiotacData()
# ff_elect_db = ff._elect_db
# ff_angle_mvavg = ff.angle_mvavg
# ff_pac1_mvavg = ff.pac1_mvavg
# ff_pdc_mvavg = ff.pdc_mvavg
# th = snn.BiotacData()
time.sleep(0.1)
# =============== Define Listen (Subscriber) Function ===============
class MouseTester(unittest.TestCase):
'''
Main classmethod
'''
def setUp(self):
# init
rospy.init_node("mouse_tester", anonymous=True)
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
client.wait_for_server()
rospy.sleep(15.)
self.data = Joy()
self.target_pose = PoseStamped()
start_pose = Pose()
self.new_pose = PoseStamped()
# expected values
self.expvals = [[[0.3, 0.0, 1.1, 0, 0.7, 0.05, 0.05],
[0.51, 0.25, 1.1, 0.7, 0.71, 0.05, 0.048],
[0.51, 0.26, 1.1, 0.7, 0.71, 0.05, 0.048]],
[[0.27, 0.25, 1.1, 0.7, 0.7, 0.05, 0.05],
[0.27, 0.35, 1.1, 0.7, 0.7, 0.05, 0.05],
[0.27, 0.35, 1.1, 0.7, 0.7, 0.05, 0.05]],
[[0.27, 0.25, 1.1, 0.7, 0.7, 0.05, 0.05],
[0.27, 0.25, 0.9, 0.7, 0.7, 0.05, 0.05],
[0.27, 0.25, 0.85, 0.7, 0.7, 0.05, 0.05]],
[[0.27, 0.25, 1.1, 0.7, 0.7, 0.05, 0.05],
[0.27, 0.25, 1.1, 0.69, 0.7, 0.05, 0.1],
[0.27, 0.25, 1, 0.7, 0.7, 0.05, 0.1]],
[[0.27, 0.25, 1.1, 0.7, 0.7, 0.05, 0.05],
[0.27, 0.25, 1.1, 0.69, 0.69, 0.1, 0.1],
[0.27, 0.25, 1.1, 0.69, 0.69, 0.1, 0.1]],
[[0.27, 0.25, 1.1, 0.7, 0.7, 0.05, 0.05],
[0.27, 0.25, 1.1, 0.7, 0.6, 0.05, 0.08],
[0.27, 0.25, 1.1, 0.7, 0.6, 0.05, 0.05]]]
# initialize arm's position
self._arm_commander = SrArmCommander(name="right_arm", set_ground=True)
start_pose = self._arm_commander.get_current_pose()
self.new_pose.header.stamp = rospy.get_rostime()
self.new_pose.header.frame_id = "world"
self.new_pose.pose = start_pose
self.new_pose.pose.position.x = 0.3
self.new_pose.pose.position.y = 0.25
self.new_pose.pose.position.z = 1.1
self.new_pose.pose.orientation.x = 0.7
self.new_pose.pose.orientation.y = 0.7
self.new_pose.pose.orientation.z = 0
self.new_pose.pose.orientation.w = 0
self.sub = rospy.Subscriber("command", PoseStamped, self.comparecall)
self.pub_arm = rospy.Publisher("command", PoseStamped, queue_size=1)
self.pub = rospy.Publisher("/spacenav/joy", Joy, queue_size=1)
# initialise joy publisher
self.data.buttons = [0, 0]
self.data.axes = [0, 0, 0, 0, 0, 0]
self.reset_joy = Joy()
self.reset_joy = self.data
self.pub.publish(self.data)
# move arm to initial position
self._arm_commander.move_to_pose_value_target_unsafe(
self.new_pose, avoid_collisions=True, wait=False)
self.pub_arm.publish(self.new_pose)
# wait for two seconds to finish initialization
rospy.sleep(2)
self.tester()
def tester(self):
"publishing data"
rate = rospy.Rate(1)
for i in range(0, 6):
self.data.axes = [0, 0, 0, 0, 0, 0]
self.data.axes[i] = -0.08
for j in range(0, 3):
self.pub.publish(self.data)
self.data.axes[i] = self.data.axes[i] + 0.08
rate.sleep()
self.assertAlmostEqual(self.target_pose.pose.position.x,
abs(self.expvals[i][j][0]),
msg="poseX failed",
delta=2)
self.assertAlmostEqual(self.target_pose.pose.position.y,
abs(self.expvals[i][j][1]),
msg="poseY failed",
delta=2)
self.assertAlmostEqual(self.target_pose.pose.position.z,
abs(self.expvals[i][j][2]),
msg="poseZ failed",
delta=2)
self.assertAlmostEqual(self.target_pose.pose.orientation.x,
abs(self.expvals[i][j][3]),
msg="rotX failed",
delta=2)
self.assertAlmostEqual(self.target_pose.pose.orientation.y,
abs(self.expvals[i][j][4]),
msg="rotY failed",
delta=2)
self.assertAlmostEqual(self.target_pose.pose.orientation.z,
abs(self.expvals[i][j][5]),
msg="rotZ failed",
delta=2)
self.assertAlmostEqual(self.target_pose.pose.orientation.w,
abs(self.expvals[i][j][6]),
msg="rotW failed",
delta=2)
# reset joystic
self.reset_joy.axes = [0, 0, 0, 0, 0, 0]
self.pub.publish(self.reset_joy)
def comparecall(self, data):
'''
checks the output of the controller
'''
self.target_pose = data
#!/usr/bin/env python
# This is an example for manually saving the queries
import rospy
from sr_robot_commander.sr_arm_commander import SrArmCommander
rospy.init_node("basic_right_arm_example", anonymous=True)
arm_commander = SrArmCommander(set_ground=False)
rospy.sleep(rospy.Duration(2))
keys = [
'ra_shoulder_pan_joint', 'ra_shoulder_lift_joint', 'ra_elbow_joint',
'ra_wrist_1_joint', 'ra_wrist_2_joint', 'ra_wrist_3_joint'
]
pose = [0.11, 0.001, -0.93, -2.22, -1.71, -1.68]
joints_goal = dict(zip(keys, pose))
print("Moving arm to joints position:\n" + str(joints_goal) + "\n")
arm_commander.move_to_joint_value_target_unsafe(joints_goal)
rospy.sleep(rospy.Duration(5))
print("Arm joints position:\n" + str(arm_commander.get_current_state()) + "\n")
raw_input("press a key to continue")
pose = [1.55, -1.15, 2.01, 2.39, -1.55, -1.58]
joints_goal = dict(zip(keys, pose))
print("Moving arm to joints position:\n" + str(joints_goal) + "\n")
arm_commander.move_to_joint_value_target_unsafe(joints_goal)
rospy.sleep(rospy.Duration(5))
Author: Chad Colestock
Date: 08/29/2015
Ver: 1.0
"""
#import numpy as np
import rospy
from sr_robot_commander.sr_hand_commander import SrHandCommander
from sr_robot_commander.sr_arm_commander import SrArmCommander
import roslib; roslib.load_manifest('sr_ronex_examples')
from sr_ronex_msgs.msg import GeneralIOState
rospy.init_node('Synergy_move_with_pot', anonymous = True)
rospy.sleep(1.0)
hand_commander = SrHandCommander()
arm_commander = SrArmCommander()
t = None
#told = 0;
def callback(data):
global t
analogue = data.analogue[0];
t = float(int((analogue+1)))/3400
#if t < told:
# t = told
# told = t;
# print (t)
# User input here:::
# more details.
#
# You should have received a copy of the GNU General Public License along
# with this program. If not, see <http://www.gnu.org/licenses/>.
# An example of how to print the current end effector of the hand and arm
# For more information, please see:
# https://dexterous-hand.readthedocs.io/en/latest/user_guide/2_software_description.html#robot-commander
from __future__ import absolute_import
import rospy
from sr_robot_commander.sr_hand_commander import SrHandCommander
from sr_robot_commander.sr_arm_commander import SrArmCommander
from sr_utilities.hand_finder import HandFinder
from math import pi
import argparse
rospy.init_node("print_ef_position", anonymous=True)
hand_commander = SrHandCommander(name="right_hand")
arm_commander = SrArmCommander(name="right_arm")
rospy.loginfo("The end effectors:\n")
eff_pose_arm = arm_commander.get_current_pose()
rospy.loginfo("The arm end effector pose:\n" + str(eff_pose_arm))
eff_pose_hand = arm_commander.get_current_pose("rh_palm")
rospy.loginfo("The hand end effector pose:\n" + str(eff_pose_hand))
# 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 sr_robot_commander.sr_arm_commander import SrArmCommander
from builtins import input
rospy.init_node("right_hand_arm_ef_pos", 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 arm separately
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(2.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
}
from __future__ import absolute_import
import rospy
from builtins import input
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from sr_robot_commander.sr_robot_commander import SrRobotCommander
from sr_robot_commander.sr_hand_commander import SrHandCommander
from sr_robot_commander.sr_arm_commander import SrArmCommander
rospy.init_node("left_hand_arm_ef_pos", 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="left_hand")
arm_commander = SrArmCommander(name="left_arm")
# How to command the arm and hand together
robot_commander = SrRobotCommander(name="left_arm_and_hand")
arm_commander.set_max_velocity_scaling_factor(0.1)
rospy.sleep(2.0)
arm_home_joints_goal = {
'la_shoulder_pan_joint': 1.47,
'la_elbow_joint': -1.695,
'la_shoulder_lift_joint': -1.22,
'la_wrist_1_joint': -01.75,
'la_wrist_2_joint': 1.57,
'la_wrist_3_joint': -1.830
}
example_pose_1 = [0.6, 0.3, 1.5, -0.7, 0.04, -0.67]
# roslaunch sr_left_ur10arm_hand.launch external_control_loop:=true sim:=false scene:=true start_home:=true
# simulated robot:
# roslaunch sr_left_ur10arm_hand.launch sim:=true scene:=true start_home:=true
# PLEASE NOTE: No scene is used in this script and fast movements of the robot. Use on real robot at your own risk.
# It is recommended to run this script in simulation first.
from __future__ import absolute_import
import rospy
from sr_robot_commander.sr_arm_commander import SrArmCommander
from sr_robot_commander.sr_hand_commander import SrHandCommander
rospy.init_node("basic_arm_examples", anonymous=True)
hand_commander = SrHandCommander(name="left_hand", prefix="lh")
arm_commander = SrArmCommander(name="left_arm", set_ground=False)
# Specify goals for hand and arm
hand_joint_goals_1 = {'lh_RFJ2': 1.59, 'lh_RFJ3': 1.49, 'lh_RFJ1': 1.47, 'lh_RFJ4': -0.01, 'lh_LFJ4': 0.02,
'lh_LFJ5': 0.061, 'lh_LFJ1': 1.41, 'lh_LFJ2': 1.60, 'lh_LFJ3': 1.49, 'lh_THJ2': 0.64,
'lh_THJ3': -0.088, 'lh_THJ1': 0.43, 'lh_THJ4': 0.49, 'lh_THJ5': 0.35, 'lh_FFJ4': -0.02,
'lh_FFJ2': 1.71, 'lh_FFJ3': 1.49, 'lh_FFJ1': 1.25, 'lh_MFJ3': 1.49, 'lh_MFJ2': 1.66,
'lh_MFJ1': 1.31, 'lh_MFJ4': -0.02}
hand_joint_goals_2 = {'lh_RFJ2': 0.55, 'lh_RFJ3': 0.08, 'lh_RFJ1': 0.03, 'lh_RFJ4': -0.15, 'lh_LFJ4': -0.35,
'lh_LFJ5': 0.23, 'lh_LFJ1': 0.02, 'lh_LFJ2': 0.49, 'lh_LFJ3': -0.02, 'lh_THJ2': -0.08,
'lh_THJ3': -0.08, 'lh_THJ1': 0.15, 'lh_THJ4': 0.56, 'lh_THJ5': -0.17, 'lh_FFJ4': -0.34,
'lh_FFJ2': 0.30, 'lh_FFJ3': 0.16, 'lh_FFJ1': 0.01, 'lh_MFJ3': 0.19, 'lh_MFJ2': 0.50,
'lh_MFJ1': 0.00, 'lh_MFJ4': -0.07}
hand_joint_goals_3 = {'lh_RFJ2': 0.63, 'lh_RFJ3': 0.77, 'lh_RFJ1': 0.033, 'lh_RFJ4': -0.02, 'lh_LFJ4': -0.32,
import rospy
import sys
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
}
#!/usr/bin/env python
# This example demonstrates some of the functions of the arm commander.
# The arm is moved through a sequence of goals generated via different functions in the commander.
import rospy
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from sr_robot_commander.sr_arm_commander import SrArmCommander
import geometry_msgs.msg
rospy.init_node("move_arm_following_waypoint_trajectory", anonymous=True)
arm_commander = SrArmCommander(set_ground=True)
rospy.sleep(rospy.Duration(1))
# Moving arm to initial pose
pose_1 = [0.32, 0.27, 1.026, 0.0, 0.0, 0.0, 1.0]
print "Moving to initial pose"
arm_commander.plan_to_pose_target(pose_1)
arm_commander.execute()
rospy.sleep(rospy.Duration(2))
print "Following trajectory defined by waypoints"
waypoints = []
# start with the initial position
initial_pose = arm_commander.get_current_pose()
waypoints.append(initial_pose)
# This example demonstrates how to move the left hand and arm through a sequence of joint goals.
# At the start and end of the sequence, both the hand and arm will spend 20s in teach mode,
# This allows the user to manually move the hand and arm. New goals can be easily generated
# using the script 'sr_print_joints_position.py
# PLEASE NOTE: move_to_joint_value_target_unsafe is used in this script, so collision checks
# between the hand and arm are not made!
import rospy
from sr_robot_commander.sr_arm_commander import SrArmCommander
from sr_robot_commander.sr_hand_commander import SrHandCommander
rospy.init_node("basic_hand_arm_example", anonymous=True)
hand_commander = SrHandCommander(name="left_hand", prefix="lh")
arm_commander = SrArmCommander(name="left_arm", set_ground=False)
# sleep for some time during which the arm can be moved around by pushing it
# but be careful to get away before the time runs out. You are warned
rospy.loginfo("Set arm teach mode ON")
arm_commander.set_teach_mode(True)
rospy.sleep(20.0)
rospy.loginfo("Set arm teach mode OFF")
arm_commander.set_teach_mode(False)
# Specify goals for hand and arm
hand_joints_goal_1 = {
'lh_FFJ1': 0.35,
'lh_FFJ2': 0.0,
'lh_FFJ3': 0.0,
# This example demonstrates how to move the right hand and arm through a sequence of joint goals.
# At the start and end of the sequence, both the hand and arm will spend 20s in teach mode,
# This allows the user to manually move the hand and arm. New goals can be easily generated
# using the script 'sr_print_joints_position.py
# PLEASE NOTE: move_to_joint_value_target_unsafe is used in this script, so collision checks
# between the hand and arm are not made!
import rospy
from sr_robot_commander.sr_arm_commander import SrArmCommander
from sr_robot_commander.sr_hand_commander import SrHandCommander
rospy.init_node("basic_hand_arm_example", anonymous=True)
hand_commander = SrHandCommander()
arm_commander = SrArmCommander()
# sleep for some time during which the arm can be moved around by pushing it
# but be careful to get away before the time runs out. You are warned
rospy.loginfo("Set arm teach mode ON")
arm_commander.set_teach_mode(True)
rospy.sleep(20.0)
rospy.loginfo("Set arm teach mode OFF")
arm_commander.set_teach_mode(False)
# Specify goals for hand and arm
hand_joints_goal_1 = {
'rh_FFJ1': 0.35,
'rh_FFJ2': 0.0,
'rh_FFJ3': 0.0,