def _init_movo_groups(self):
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.lmove_group = MoveGroupInterface("left_arm", "base_link")
self.rmove_group = MoveGroupInterface("right_arm", "base_link")
self.move_group.setPlannerId("RRTConnectkConfigDefault")
self.lmove_group.setPlannerId("RRTConnectkConfigDefault")
self.rmove_group.setPlannerId("RRTConnectkConfigDefault")
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
group_names = self.robot.get_group_names()
print("Avaliable Groups_name", group_names)
self.movegroup_upper_group = moveit_commander.MoveGroupCommander(
"upper_body")
self.movegroup_rarm_group = moveit_commander.MoveGroupCommander(
"right_arm")
self.movegroup_larm_group = moveit_commander.MoveGroupCommander(
"left_arm")
# # time.sleep(15)
# # time.sleep(15)
self.display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20) # For trajectory publish in rviz display
print("All groups are inited!")
def __init__(self):
self.group = MoveGroupInterface("arm", "base_link")
self.gripper = MoveGroupInterface("gripper", "base_link")
self.virtual_state = RobotState()
self.trajectory = None
# Sleep to allow RVIZ time to start up.
print "============ Waiting for RVIZ..."
rospy.sleep(5)
print "============ Starting planning"
def __init__(self):
self.group = MoveGroupInterface("arm", "base_link", plan_only=True)
self.gripper = MoveGroupInterface("gripper",
"base_link",
plan_only=True)
self.virtual_arm_state = RobotState()
self.virtual_gripper_state = RobotState()
# Sleep to allow RVIZ time to start up.
print "============ Waiting for RVIZ..."
rospy.sleep(5)
print "============ Starting planning"
def __init__(self):
# self.scene = PlanningSceneInterface("base_link")
self.dof = "7dof"
self.robot = moveit_commander.RobotCommander()
self._listener = tf.TransformListener()
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.lmove_group = MoveGroupInterface("left_arm", "base_link")
self.rmove_group = MoveGroupInterface("right_arm", "base_link")
self.move_group.setPlannerId("RRTConnectkConfigDefault")
self.lmove_group.setPlannerId("RRTConnectkConfigDefault")
self.rmove_group.setPlannerId("RRTConnectkConfigDefault")
if "7dof" == self.dof:
self._upper_body_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint",
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint"
]
self.tucked = [
-1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7,
0.0, -0.5, 1.7, 0.04, 0, 0
]
# self.constrained_stow =[-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0, 0.0, -1.0, 0.42, 0, 0]
self.constrained_stow = [
-2.037, 1.046, -2.877, -1.423, -1.143, 1.679, 1.690, 2.037,
-1.046, 2.877, 1.423, 1.143, -1.679, -1.690, 0.4, 0.288, 0
]
else:
rospy.logerr("DoF needs to be set at 7, aborting demo")
return
self._lgripper = GripperActionClient('left')
self._rgripper = GripperActionClient('right')
def gotoInit(self):
move_group = MoveGroupInterface("arm", "base_link")
planning_scene = PlanningSceneInterface("base_link")
joint_names = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
# Plans the joints in joint_names to angles in pose
init = self.trajActionGoal.trajectory.points[0].positions
move_group.moveToJointPosition(joint_names, init, wait=False)
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Moved to Init")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
def __init__(self):
self.moveGroup = MoveGroupInterface("arm_with_torso", "base_link")
self.planning_scene = PlanningSceneInterface("base_link")
self.planning_scene.addCube("my_front_ground", 2, 1.4, 0.0, -0.48)
self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -0.6)
self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -0.6)
self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -0.6)
def __init__(self):
self.bfp = True
self.robot = robot_interface.Robot_Interface()
self.url = 'http://172.31.76.30:80/ThinkingQ/'
self.joint_names = ["shoulder_pan_joint",
"shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"]
self.tf_listener = TransformListener()
trfm = Transformer()
self.r2b = trfm.transform_matrix_of_frames(
'head_camera_rgb_optical_frame', 'base_link')
self.model = load_model("./model/0.988.h5")
self.br = TransformBroadcaster()
self.move_group = MoveGroupInterface("arm", "base_link")
# self.pose_group = moveit_commander.MoveGroupCommander("arm")
self.cam = camera.RGBD()
self.position_cloud = None
while True:
try:
cam_info = self.cam.read_info_data()
if cam_info is not None:
break
except:
rospy.logerr('camera info not recieved')
self.pcm = PCM()
self.pcm.fromCameraInfo(cam_info)
def set_init_pose(self):
#set init pose
move_group = MoveGroupInterface(
"manipulator", "base_link") #MoveGroupInterface のインスタンスの作成
planning_scene = PlanningSceneInterface(
"base_link") #PlanningSceneInterface のインスタンスの作成
joint_names = [
"shoulder_pan_joint",
"shoulder_lift_joint", #ジョイントの名前を定義
"elbow_joint",
"wrist_1_joint",
"wrist_2_joint",
"wrist_3_joint"
]
# pose = [-1.26 , -0.64 , -2.44 , -0.66 , 1.56 , 0.007]
# move_group.moveToJointPosition(joint_names, pose, wait=False)#joint_names を pose に動かす
# move_group.get_move_action().wait_for_result() #wait result
# result = move_group.get_move_action().get_result() #result を代入
move_group.get_move_action().cancel_all_goals() #すべてのゴールをキャンセル
self.state = {
'default': 0,
'hold': 1,
'way_standby': 2,
'plan_standby': 3
}
# default : 何もしていない target のセットか hold button を待つ
# hold : waypoint のセットを受け付ける hold button が離れたら終了
#way_standby : execute button が押されるのを待つ 押されたら default へ waypoint の設置をした場合
#plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合
self.now_state = self.state['default'] #現在フレームの状態
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
grasp_topic = "fetch_grasp_planner_node/plan"
rospy.loginfo("Waiting for %s..." % grasp_topic)
self.grasp_planner_client = actionlib.SimpleActionClient(
grasp_topic, GraspPlanningAction)
wait = self.grasp_planner_client.wait_for_server(rospy.Duration(5))
if (wait):
print("successfully connected to grasp server")
else:
print("failed to connect to grasp server")
rospy.Subscriber("fetch_fruit_harvest/apple_pose", Pose,
self.apple_pose_callback)
self.pub = rospy.Publisher("fetch_fruit_harvest/grasp_msg",
String,
queue_size=10)
self.object = Object()
self.grasps = Grasp()
self.ready_for_grasp = False
self.pick_place_finished = False
self.first_time_grasp = True
self.scene.addBox("ground", 300, 300, 0.02, 0, 0, 0)
self.tuck()
def _home_arm_planner(self):
if self._prefix == 'left':
rospy.sleep(5)
else:
move_group_jtas = MoveGroupInterface("upper_body", "base_link")
move_group_jtas.setPlannerId("RRTConnectkConfigDefault")
success = False
while not rospy.is_shutdown() and not success:
result = move_group_jtas.moveToJointPosition(
self._body_joints, self._homed, 0.05)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.logerr("_home_arm_planner completed ")
success = True
else:
rospy.logerr(
"_home_arm_planner: _home_arm_planner failed (%d)" %
result.error_code.val)
self._arms_homing = True
self._ctl.api.MoveHome()
self._ctl.api.InitFingers()
self.home_arm_pub.publish(Bool(True))
rospy.sleep(2.0)
self._arms_homing = False
self._planner_homing = False
def __init__(self):
self.group = MoveGroupInterface("arm", "base_link")
# Sleep to allow RVIZ time to start up.
print "============ Waiting for RVIZ..."
rospy.sleep(15)
print "============ Starting planning"
def tuck():
if not is_moveit_running():
rospy.loginfo("starting moveit")
move_thread = MoveItThread()
rospy.loginfo("Waiting for MoveIt...")
client = MoveGroupInterface("arm_with_torso", "base_link")
rospy.loginfo("...connected")
# Padding does not work (especially for self collisions)
# So we are adding a box above the base of the robot
scene = PlanningSceneInterface("base_link")
scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375)
joints = ["torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = client.moveToJointPosition(joints,
pose,
0.0,
max_velocity_scaling_factor=0.5)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
scene.removeCollisionObject("keepout")
move_thread.stop()
return
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
# 初始化需要使用move group控制的机械臂中的arm group
self.arm = MoveGroupCommander('arm')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
self.arm.set_goal_position_tolerance(0.01)
self.arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
self.arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
self.arm.set_pose_reference_frame(reference_frame)
# 设置每次运动规划的时间限制：5s
self.arm.set_planning_time(5)
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def __init__(self):
self.move_group = MoveGroupInterface("arm_with_torso", "base_link")
self.pose_group = moveit_commander.MoveGroupCommander("arm_with_torso")
self.joint_names = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
# planning_scene = PlanningSceneInterface("base_link")
# planning_scene.removeCollisionObject("my_front_ground")
# planning_scene.removeCollisionObject("my_back_ground")
# planning_scene.removeCollisionObject("my_right_ground")
# planning_scene.removeCollisionObject("my_left_ground")
# planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
# planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
# planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
# planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
self.tuck_the_arm_joints_value = [
0, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0
]
self.stow_joints_value = [0.3, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
# self.high_pose_value = [0.38380688428878784, -0.12386894226074219, -0.31408262252807617, 2.1759514808654785, 0.0061359405517578125, 0.9556700587272644, -1.921694278717041, -1.6908303499221802]
self.high_pose_value = [
0.4946120488643647, -0.19136428833007812, -0.4793691635131836,
0.009587380103766918, 0.1629854440689087, 0.2983585298061371,
1.8430781364440918, -1.6992675065994263
]
self.trans = Transformer()
def main():
arm_move_group = MoveGroupInterface("arm", "base_link", plan_only=True)
arm_trajectory_client = actionlib.SimpleActionClient(
"arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
arm_trajectory_client.wait_for_server()
argi = 0
while argi < len(sys.argv):
arg = sys.argv[argi]
argi += 1
if arg == 'arm':
print('Contacting move_group for arm...', file=sys.stderr)
arm_move_group = MoveGroupInterface("arm",
"base_link",
plan_only=True)
arm_trajectory_client = actionlib.SimpleActionClient(
"arm_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
arm_trajectory_client.wait_for_server()
joints = [
'shoulder_pan_joint',
'shoulder_lift_joint',
'upperarm_roll_joint',
'elbow_flex_joint',
'forearm_roll_joint',
'wrist_flex_joint',
'wrist_roll_joint',
]
pose = [float(a) for a in sys.argv[argi:argi + len(joints)]]
argi += len(joints)
result = self.arm_move_group.moveToJointPosition(joints,
pose,
plan_only=True)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = result.planned_trajectory.joint_trajectory
print('sending trajectory to arm...', file=sys.stderr)
self.arm_trajectory_client.send_goal(follow_goal)
result = self.arm_trajectory_client.wait_for_result()
print('arm followed trajectory %s' % result, file=sys.stderr)
else:
logger.warn('moveToJointPosition returned %s', result)
return
else:
print('Unknown joint group %s' % arg, file=sys.stderr)
return
def __init__(self):
self.client = actionlib.SimpleActionClient(
"gripper_controller/gripper_action", GripperCommandAction)
rospy.loginfo("Waiting for gripper_action...")
self.move_group = MoveGroupInterface("arm_with_torso", "base_link")
self.goal = GripperCommandGoal()
self.client.wait_for_server()
def main():
mgi = MoveGroupInterface("l_arm", "benderbase_link", None,
False) #"bender/l_shoulder_pitch_link"
tip_link = "benderl_wrist_pitch_link"
joint_names = [
'l_shoulder_pitch_joint', 'l_shoulder_roll_joint',
'l_shoulder_yaw_joint', 'l_elbow_pitch_joint', 'l_elbow_yaw_joint',
'l_wrist_pitch_joint'
]
rospy.loginfo('STARTING TEST')
count = 0
succ_plan = 0
home = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
mgi.moveToJointPosition(joint_names, home)
rospy.loginfo('POSITION: home')
rospy.sleep(2.0)
#---------------------------------------------------------------------------------------------------------------------------------------------------
goal = PoseStamped()
#Position
goal.pose.position.x, goal.pose.position.y, goal.pose.position.z = 0.2, 0.1, 0.1
#simple_orientation = Quaternion(0.0,-0.70710678,0.0,0.70710678)
#Orientation
goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w = -0.0119682, -0.70163, -0.00910682, 0.712382 #-0.0148,-0.733 , 0.0, 0.678
goal.header.frame_id = "benderbase_link"
#---------------------------------------------------------------------------------------------------------------------------------------------------
#------------------------------------------------Iterarion Params-----------------------------------------------------------------------------------
dl = 0.03 #precision en metros (0.03)
Xmin = -0.623244 #-0.05
Xmax = 0.615229 #0.8
Ymin = -0.829381 #-0.4
Ymax = -0.261919 #0.05
Zmin = 0.393434 #0.55
Zmax = 1.346654 #0.85
# ~17 horas worst case scenario
#---------------------------------------------------------------------------------------------------------------------------------------------------
workspace = [] #matrix for saving coordinates and error codes
for i in my_range(Xmin, Xmax, dl):
for j in my_range(Ymin, Ymax, dl):
for k in my_range(Zmin, Zmax, dl):
goal.pose.position.x, goal.pose.position.y, goal.pose.position.z = i, j, k
error_code = Move(mgi, tip_link, goal)
if (error_code == 1): #Move(mgi,tip_link,goal)==1):
succ_plan += 1
count += 1
workspace.append([error_code, i, j, k])
percentage = (succ_plan / count) * 100
rospy.loginfo('succeeded plannings: {0}/{1} ({2}%)'.format(
succ_plan, count, percentage))
rospy.loginfo('TEST ENDED')
with open(
'/home/robotica/uchile_ws/pkgs/base_ws/bender_core/bender_test_config_v38/scripts/workspace.csv',
'wb') as f:
writer = csv.writer(f)
writer.writerows(workspace)
def __init__(self, image_topic, camera_info_topic, camera_frame,
published_point_num_topic, published_point_base_topic,
torso_movement_topic, head_movement_topic,
num_published_points, max_spine_height, min_spine_height,
spine_offset):
'''
Description: establishes the variabes included in the class
Input: self <Object>, image_topic <Object>, camera_info_topic <String>,
camera_frame <String>, published_point_num_topic <String>,
published_point_base_topic <String>, torso_movement_topic <String>,
head_movement_topic <String>, num_published_points <Int>,
max_spine_height <Int>, min_spine_height <Int>, spine_offset <Int>
Return: None
'''
self.camera_frame = camera_frame
self.published_point_base_topic = published_point_base_topic
self.bridge = CvBridge()
self.tf = TransformListener()
rospy.Subscriber(image_topic, Image, self.image_callback)
rospy.Subscriber(camera_info_topic, CameraInfo,
self.camera_info_callback)
self.robot_pose = None
self.img = None
self.pc = None
self.camera_info = None
self.num_published_points = num_published_points
self.published_points = [[0, 0, 0]
for i in range(num_published_points)]
for i in range(num_published_points):
rospy.Subscriber(self.published_point_base_topic + str(i),
PointStamped, self.point_published_callback, i)
self.points_registered = sets.Set()
# base movement
self.base_client = actionlib.SimpleActionClient(
'move_base', MoveBaseAction)
self.base_client.wait_for_server()
# torso movement
self.max_spine_height = max_spine_height
self.min_spine_height = min_spine_height
self.spine_offset = spine_offset
# head movement
self.point_head_client = actionlib.SimpleActionClient(
head_movement_topic, PointHeadAction)
self.point_head_client.wait_for_server()
# keyboard listener
self.keypress_sub = rospy.Subscriber('/key_monitor', String,
self.key_callback)
rospy.loginfo("move group")
self.move_group = MoveGroupInterface("arm_with_torso", "base_link")
rospy.loginfo("move group end")
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def tuck():
move_group = MoveGroupInterface("arm", "base_link")
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = move_group.moveToJointPosition(joints, pose, 0.02)
def move():
rospy.init_node("move")
move_group = MoveGroupInterface("arm_with_torso", "base_link")
#planning scene setup
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
#move head camera
client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction)
rospy.loginfo("Waiting for head_controller...")
client.wait_for_server()
goal = PointHeadGoal()
goal.target.header.stamp = rospy.Time.now()
goal.target.header.frame_id = "base_link"
goal.target.point.x = 1.2
goal.target.point.y = 0.0
goal.target.point.z = 0.0
goal.min_duration = rospy.Duration(1.0)
client.send_goal(goal)
client.wait_for_result()
#arm setup
move_group = MoveGroupInterface("arm", "base_link")
#set arm initial position
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def arm_move(self):
move_group = MoveGroupInterface("arm_with_torso", "base_link")
# Define ground plane
# This creates objects in the planning scene that mimic the ground
# If these were not in place gripper could hit the ground
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
#planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_left_ground", 1, 1.5,
self.translation[2][0], self.translation[2][1],
self.translation[2][2])
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# TF joint names
joint_names = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
# Lists of joint angles in the same order as in joint_names
disco_poses = [[0, 2.5, -0.1, 3.0, 1.5, 3.0, 1.0, 3.0]]
for pose in disco_poses:
if rospy.is_shutdown():
break
# Plans the joints in joint_names to angles in pose
move_group.moveToJointPosition(joint_names, pose, wait=False)
# Since we passed in wait=False above we need to wait here
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Disco!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
# This stops all arm movement goals
# It should be called when a program is exiting so movement stops
move_group.get_move_action().cancel_all_goals()
def __init__(self):
self.move_delay = 0.2
self.lastpose = []
self.lasttime = time.time()
self.need_plan = True
self.joint_names = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
self.move_group = MoveGroupInterface("arm_with_torso", "base_link")
def main(args):
rospy.init_node('simple_move')
move_group = MoveGroupInterface('manipulator', "base_link")
planning_scene = PlanningSceneInterface("base_link")
joint_names = [
'shoulder_1_joint', 'shoulder_2_joint', 'elbow_1_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
# This is the wrist link not the gripper itself
gripper_frame = "wrist_3_link"
poses, pose_name = get_pose(args.reset)
# Construct a "pose_stamped" message as required by moveToPose
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = "base_link"
while not rospy.is_shutdown():
for pose in poses:
# Finish building the Pose_stamped message
# If the message stampe is not current it could be ignored
gripper_pose_stamped.header.stamp = rospy.Time.now()
# Set the message pose
gripper_pose_stamped.pose = pose
move_group.moveToPose(gripper_pose_stamped, gripper_frame)
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
log_output = pose_name + " Done!"
rospy.loginfo(log_output)
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
# This stops all arm movement goals
# It should be called when a program is exiting so movement stops
move_group.get_move_action().cancel_all_goals()
def set_init_pose(sefl):
#set init pose
move_group = MoveGroupInterface("manipulator","base_link")
planning_scene = PlanningSceneInterface("base_link")
joint_names = ["shoulder_pan_joint", "shoulder_lift_joint",
"elbow_joint", "wrist_1_joint",
"wrist_2_joint", "wrist_3_joint"]
pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0]
move_group.moveToJointPosition(joint_names, pose, wait=False)
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
move_group.get_move_action().cancel_all_goals()
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
##self.objectname_sub= rospy.Subscriber("/objects",Float32MultiArray,self.callback)
##self.interestedObjectName = ""
#self.objects = []
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def __init__(self):
self.moveGroup = MoveGroupInterface("arm_with_torso", "base_link")
self.planning_scene = PlanningSceneInterface("base_link")
self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
self.planning_scene.removeCollisionObject("my_front_ground")
self.planning_scene.removeCollisionObject("my_back_ground")
self.planning_scene.removeCollisionObject("my_right_ground")
self.planning_scene.removeCollisionObject("my_left_ground")
self.planning_scene.removeCollisionObject("table1")
def set_init_pose(self):
#set init pose
move_group = MoveGroupInterface("manipulator","base_link") #MoveGroupInterface のインスタンスの作成
planning_scene = PlanningSceneInterface("base_link") #PlanningSceneInterface のインスタンスの作成
joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", #ジョイントの名前を定義
"elbow_joint", "wrist_1_joint",
"wrist_2_joint", "wrist_3_joint"]
pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0]
move_group.moveToJointPosition(joint_names, pose, wait=False)#joint_names を pose に動かす
move_group.get_move_action().wait_for_result() #wait result
result = move_group.get_move_action().get_result() #result を代入
move_group.get_move_action().cancel_all_goals() #すべてのゴールをキャンセル
def __init__(self):
rospy.loginfo("Waiting for Fetch ...")
self.tuck_the_arm_joints_value = [
0, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0
]
self.stow_joints_value = [0.0, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
self.move_group = MoveGroupInterface("arm_with_torso", "base_link")
self.pose_group = moveit_commander.MoveGroupCommander("arm_with_torso")
self.joint_names = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
def set_init_pose(self):
#set init pose
move_group = MoveGroupInterface("arm_with_torso", "base_link")
#set arm initial position
joints = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
#pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
pose = [0.385, -1.0, -1.0, 0.0, 1.0, 0.0, 1.5, 0.0]
#pose = [0.385, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
result = move_group.moveToJointPosition(joints, pose, 0.02)
