def __init__(self):
#关于baxter无法通过moveit获取当前姿态的错误 https://github.com/ros-planning/moveit/issues/1187
#joint_state_topic = ['joint_states:=/robot/joint_states']
#初始化moveit的 API接口
moveit_commander.roscpp_initialize(sys.argv)
#初始化ros节点，
rospy.init_node('panda_grasp', anonymous=True)
#构建一个tf发布器
self.tf_broadcaster = tf.TransformBroadcaster()
self.grasp_config = GraspConfig()
#创建一个TF监听器
self.tf_listener = tf.TransformListener()
#一直等待接收到桌面标签和机器人base坐标系之间的变换（需要提前进行手眼标定）
get_transform = False
while not get_transform:
try:
#尝试查看机器人基座base与桌面标签之间的转换
trans, rot = self.tf_listener.lookupTransform(
'/panda_link0', '/ar_marker_6', rospy.Time(0))
euler = tf.transformations.euler_from_quaternion(rot)
self.base2marker = tf.transformations.compose_matrix(
translate=trans, angles=euler)
#查看gripper到link8之间的变换
trans, rot = self.tf_listener.lookupTransform(
'/panda_EE', '/panda_link8', rospy.Time(0))
euler = tf.transformations.euler_from_quaternion(rot)
self.gripper2link8 = tf.transformations.compose_matrix(
translate=trans, angles=euler)
#查看base到panda_link8的变换，此时就是查询gripper的初始姿态
trans, rot = self.tf_listener.lookupTransform(
'/panda_link0', '/panda_link8', rospy.Time(0))
euler = tf.transformations.euler_from_quaternion(rot)
self.base2Initial_link8 = tf.transformations.compose_matrix(
translate=trans, angles=euler)
get_transform = True
rospy.loginfo("got transform complete")
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.loginfo("got transform failed")
rospy.sleep(0.5)
continue
# 初始化场景对象
scene = moveit_commander.PlanningSceneInterface()
rospy.sleep(2)
# 创建机械臂规划组对象
self.selected_arm = moveit_commander.MoveGroupCommander('panda_arm')
#创建机械手规划对象
hand_group = moveit_commander.MoveGroupCommander('hand')
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
DisplayTrajectory,
queue_size=20)
# 获取左臂末端执行器名称
self.end_effector_link = self.selected_arm.get_end_effector_link()
print("i am here")
print(self.end_effector_link)
# 创建机械臂父坐标系名称字符
reference_frame = 'panda_link0'
#self.tf_listener = tf.TransformListener()
# 设置父坐标系名称
#self.selected_arm.set_pose_reference_frame(reference_frame)
# 允许机械臂进行重规划
#self.selected_arm.allow_replanning(True)
# 允许机械臂末位姿的错误余量
self.selected_arm.set_goal_position_tolerance(0.01)
self.selected_arm.set_goal_orientation_tolerance(0.05)
#不允许规划失败重规划,规划时间只允许5秒钟,否则很浪费时间
self.selected_arm.allow_replanning(False)
self.selected_arm.set_planning_time(5)
#清除之前遗留的物体
scene.remove_world_object('table')
#设置桌面高度
table_ground = 0.6
#桌面尺寸 x y z
table_size = [0.6, 1.2, 0.01]
# 将table加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = 'panda_link0'
table_pose.pose.position.x = 0.55
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = 0.025
table_pose.pose.orientation.w = 1.0
scene.add_box('table', table_pose, table_size)
#设置初始姿态
#Home_positions = [-0.08, -1.0, -1.19, 1.94, 0.67, 1.03, -0.50]#移动到工作位置，使用正运动学
# Set the arm's goal configuration to the be the joint positions
#self.selected_arm.set_joint_value_target(Home_positions)
# Plan and execute the motion
#self.selected_arm.go()
#rospy.sleep(5)
# 设置home姿态
Home_positions = [0.04, -0.70, 0.18, -2.80, 0.19, 2.13,
0.92] #移动到工作位置，使用正运动学
#self.selected_arm.remember_joint_values('resting', joint_positions)#存储当前状态为初始状态
#self.start_state =self.selected_arm.get_current_pose()
# Set the arm's goal configuration to the be the joint positions
self.selected_arm.set_joint_value_target(Home_positions)
# Plan and execute the motion，运动到Home位置
self.selected_arm.go()
self.selected_arm.stop()
joint_goal = hand_group.get_current_joint_values()
joint_goal[0] = 0.04
joint_goal[1] = 0.04
hand_group.go(joint_goal, wait=True)
hand_group.stop()
######################开始等待接收夹爪姿态#########################
print("Waiting for gripper pose!")
#等待gripper_pose这个话题的发布（目标抓取姿态，该姿态将会进行抓取）
#rospy.wait_for_message('/detect_grasps/clustered_grasps', GraspConfigList)
#创建消息订阅器，订阅“gripper_pose”话题，这个gripper_pose，是以桌面标签为参考系的
#接收到之后，调用回调函数，有两件事要做
# 1.将gripper_pose
# 计算后撤距离
self.r_flag = False
rospy.Subscriber('/detect_grasps/clustered_grasps',
GraspConfigList,
self.Callback,
queue_size=1)
#能不能，先检查并读取 桌面标签和机器人底座之间的转换关系？（这只是其中一条路吧）
#利用TF直接，读取grasp_pose在base坐标系下面的姿态
#调用pose处理函数，计算预抓取姿态，
#rospy.sleep(5)
print("Start to Recieve Grasp Config!")
#####################################################################
while not rospy.is_shutdown():
if self.r_flag:
#目标姿态设定为预抓取位置
#self.pre_grasp_pose
self.r_flag = False
#尝试利用TF直接，读取grasp_pose在base坐标系下面的姿态
#计算预抓取姿态
else:
rospy.sleep(0.5)
continue
#以当前姿态作为规划起始点
self.selected_arm.set_start_state_to_current_state()
# 对末端执行器姿态设定目标姿态
#self.selected_arm.set_pose_target(target_pose, 'left_gripper')
# 规划轨迹
#traj = self.selected_arm.plan(target_pose.pose)
# 执行轨迹，运行到预抓取位置
#self.selected_arm.execute(traj)
print(self.end_effector_link)
print('Moving to pre_grasp_pose')
#self.selected_arm.pick("test",self.grasp_config,plan_only = True)
#traj=self.selected_arm.plan(self.pre_grasp_pose)
#self.selected_arm.set_pose_target(self.pre_grasp_pose,end_effector_link="panda_EE")
#traj=self.selected_arm.plan()
#continue
#success=self.selected_arm.execute(traj)
#print(target_pose.pose)
#设置规划
#self.selected_arm.set_planning_time(5)
success = self.selected_arm.go(self.pre_grasp_pose, wait=True)
self.selected_arm.stop()
self.selected_arm.clear_pose_targets()
if not success:
print('Failed to move to pre_grasp_pose!')
continue
print('Move to pre_grasp_pose succeed')
#等待机械臂稳定
rospy.sleep(1)
#再设置当前姿态为起始姿态
self.selected_arm.set_start_state_to_current_state()
#
waypoints = []
wpose = self.selected_arm.get_current_pose().pose
#print("#####wpose.position")
#print(wpose.position)
#print("#####self.grasp_pose2")
#print(self.grasp_pose.position)
wpose.position.x = self.grasp_pose.position.x
wpose.position.y = self.grasp_pose.position.y
wpose.position.z = self.grasp_pose.position.z
waypoints.append(copy.deepcopy(wpose))
#wpose = self.selected_arm.get_current_pose().pose
#wpose.position.z -= scale * 0.1
#规划从当前位姿，保持姿态，转移到目标夹爪姿态的路径
(plan, fraction) = self.selected_arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
##显示轨迹
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = self.selected_arm.get_current_state(
)
display_trajectory.trajectory.append(plan)
# Publish
display_trajectory_publisher.publish(display_trajectory)
#执行,并等待这个轨迹执行成功
new_plan = self.scale_trajectory_speed(plan, 0.3)
self.selected_arm.execute(new_plan, wait=True)
#self.selected_arm.shift_pose_target(2,0.05,"panda_link8")
#self.selected_arm.go()
#执行抓取
rospy.sleep(2)
print("Grasping")
joint_goal = hand_group.get_current_joint_values()
joint_goal[0] = 0.015
joint_goal[1] = 0.015
#plan=hand_group.plan(joint_goal)
#new_plan=self.scale_trajectory_speed(plan,0.3)
hand_group.go(joint_goal, wait=True)
hand_group.stop()
####################抓取完后撤####################
waypoints = []
wpose = self.selected_arm.get_current_pose().pose
wpose.position.x = self.pre_grasp_pose.position.x
wpose.position.y = self.pre_grasp_pose.position.y
wpose.position.z = self.pre_grasp_pose.position.z
waypoints.append(copy.deepcopy(wpose))
#规划从当前位姿，保持姿态，转移到目标夹爪姿态的路径
(plan, fraction) = self.selected_arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
#执行,并等待后撤成功
new_plan = self.scale_trajectory_speed(plan, 0.6)
self.selected_arm.execute(new_plan, wait=True)
"""
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = self.selected_arm.get_current_state()
display_trajectory.trajectory.append(plan)
# Publish
display_trajectory_publisher.publish(display_trajectory)
"""
######################暂时设置直接回到Home############################
#self.selected_arm.remember_joint_values('resting', joint_positions)#存储当前状态为初始状态
#self.start_state =self.selected_arm.get_current_pose(self.end_effector_link)
# Set the arm's goal configuration to the be the joint positions
self.selected_arm.set_joint_value_target(Home_positions)
# Plan and execute the motion，运动到Home位置
self.selected_arm.go()
self.selected_arm.stop()
joint_goal = hand_group.get_current_joint_values()
joint_goal[0] = 0.04
joint_goal[1] = 0.04
hand_group.go(joint_goal, wait=True)
hand_group.stop()
print("Grasp done")
rospy.sleep(5)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def cw3_example_script():
"""
This script will go through the main aspects of moveit and the components you will need to complete the coursework.
You can find more information on
"""
# Initialize moveit_commander and rospy node
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface')
# Initialize moveit scene interface (woeld surrounding the robot)
scene = moveit_commander.PlanningSceneInterface()
# Robot contains the entire state of the robot (iiw a and shadow hand)
global group, base_group
robot = moveit_commander.RobotCommander()
group = moveit_commander.MoveGroupCommander('robot')
arm_group = moveit_commander.MoveGroupCommander('arm')
gripeer_group = moveit_commander.MoveGroupCommander('gripper')
base_group = moveit_commander.MoveGroupCommander('base')
# Create a DisplayTrajectory publisher which is used later to publish trajectories for RViz to visualize:
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
group.allow_replanning(True)
group.allow_looking(True)
# Set the number of planning attempts to find better solution
group.set_num_planning_attempts(10)
################################################
###Add obstacle
rospy.sleep(2)
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "odom"
box_pose.pose.position.x = 0
box_pose.pose.position.y = 3
box_pose.pose.position.z = 0.01
box_name = "wall"
scene.add_box(box_name, box_pose, size=(2, 2, 0.01))
rospy.sleep(0.5)
#############################################################
group.set_num_planning_attempts(200)
pose = group.get_current_pose(group.get_end_effector_link())
group.set_path_constraints(None)
##########################################################
"""way point """
##########################################################
group.set_position_target([-1, 2, 0.2], group.get_end_effector_link())
result = group.plan()
# print(result.joint_trajectory.points[-1].positions)
# Move arm to avoid obstacle when printing
joint_goal = group.get_current_joint_values()
joint_goal[4:] = result.joint_trajectory.points[-1].positions[4:]
joint_goal[2] = pi / 2
joint_goal[3] = 2.9496
group.go(joint_goal)
pose = group.get_current_pose(group.get_end_effector_link())
pose.pose.position.x = -1
pose.pose.position.y = 2
pose.pose.position.z = 0.2
group.set_pose_target(pose)
group.go()
# Move the robot to the center of the striaght line to make sure Way point method can be executed
# Way points
waypoints = []
wpose = base_group.get_current_pose().pose
waypoints.append(copy.deepcopy(wpose))
wpose.position.x = -1
waypoints.append(copy.deepcopy(wpose))
wpose.position.x = 1
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = base_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
base_group.execute(plan)
###################################################################
for i in range(3):
rotation(90)
constraints = Constraints()
joint_constraint = JointConstraint()
constraints.joint_constraints.append(
JointConstraint(joint_name='arm_joint_1',
position=2.95,
tolerance_above=0.05,
tolerance_below=0.05,
weight=1))
# constraints.joint_constraints.append(JointConstraint(joint_name='z_joint',
# position = pi, tolerance_above=0.05,
# tolerance_below=0.05, weight=1))
group.set_path_constraints(constraints)
waypoints = []
wpose = group.get_current_pose().pose
waypoints.append(copy.deepcopy(wpose))
wpose.position.x = 1
wpose.position.y = 2
waypoints.append(copy.deepcopy(wpose))
wpose.position.x = 1
wpose.position.y = 4
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
group.execute(plan)
group.set_path_constraints(None)
#########################
rotation(90)
constraints = Constraints()
joint_constraint = JointConstraint()
constraints.joint_constraints.append(
JointConstraint(joint_name='arm_joint_1',
position=2.95,
tolerance_above=0.05,
tolerance_below=0.05,
weight=1))
# constraints.joint_constraints.append(JointConstraint(joint_name='z_joint',
# position = pi*3/2, tolerance_above=0.05,
# tolerance_below=0.05, weight=1))
group.set_path_constraints(constraints)
waypoints = []
wpose = group.get_current_pose().pose
waypoints.append(copy.deepcopy(wpose))
wpose.position.x = 1
wpose.position.y = 4
waypoints.append(copy.deepcopy(wpose))
wpose.position.x = -1
wpose.position.y = 4
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
group.execute(plan)
group.set_path_constraints(None)
##########################
rotation(90)
constraints = Constraints()
joint_constraint = JointConstraint()
constraints.joint_constraints.append(
JointConstraint(joint_name='arm_joint_1',
position=2.95,
tolerance_above=0.05,
tolerance_below=0.05,
weight=1))
# constraints.joint_constraints.append(JointConstraint(joint_name='z_joint',
# position = pi*2, tolerance_above=0.05,
# tolerance_below=0.05, weight=1))
group.set_path_constraints(constraints)
waypoints = []
wpose = group.get_current_pose().pose
waypoints.append(copy.deepcopy(wpose))
wpose.position.x = -1
wpose.position.y = 4
waypoints.append(copy.deepcopy(wpose))
wpose.position.x = -1
wpose.position.y = 2
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
group.execute(plan)
group.set_path_constraints(None)
########################################################################
rotation(90)
### Move the robot to initial point
group.set_position_target([-1, 2, 0.2], group.get_end_effector_link())
result = group.plan()
# print(result.joint_trajectory.points[-1].positions)
# Move arm to avoid obstacle when printing
joint_goal = group.get_current_joint_values()
joint_goal[4:] = result.joint_trajectory.points[-1].positions[4:]
joint_goal[2] = pi / 2
joint_goal[3] = 2.9496
group.go(joint_goal)
pose = group.get_current_pose(group.get_end_effector_link())
pose.pose.position.x = -1
pose.pose.position.y = 2
pose.pose.position.z = 0.2
group.set_pose_target(pose)
group.go()
### go straight line
waypoints = []
wpose = base_group.get_current_pose().pose
waypoints.append(copy.deepcopy(wpose))
wpose.position.x = -1
waypoints.append(copy.deepcopy(wpose))
wpose.position.x = 1
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = base_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
base_group.execute(plan)
print 'all finish'
# When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
def __init__(self):
super(MoveGroupPythonIntefaceTutorial, self).__init__()
## BEGIN_SUB_TUTORIAL setup
##
## First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface', anonymous=True)
## Instantiate a `RobotCommander`_ object. Provides information such as the robot's
## kinematic model and the robot's current joint states
robot = moveit_commander.RobotCommander()
## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface
## for getting, setting, and updating the robot's internal understanding of the
## surrounding world:
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a `MoveGroupCommander`_ object. This object is an interface
## to a planning group (group of joints). In this tutorial the group is the primary
## arm joints in the Panda robot, so we set the group's name to "panda_arm".
## If you are using a different robot, change this value to the name of your robot
## arm planning group.
## This interface can be used to plan and execute motions:
group_name = "my_arm"
move_group = moveit_commander.MoveGroupCommander(group_name)
## Create a `DisplayTrajectory`_ ROS publisher which is used to display
## trajectories in Rviz:
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
## END_SUB_TUTORIAL
## BEGIN_SUB_TUTORIAL basic_info
##
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
# We can get the name of the reference frame for this robot:
planning_frame = move_group.get_planning_frame()
print "============ Planning frame: %s" % planning_frame
# We can also print the name of the end-effector link for this group:
eef_link = move_group.get_end_effector_link()
print "============ End effector link: %s" % eef_link
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print "============ Available Planning Groups:", robot.get_group_names()
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print "============ Printing robot state"
print robot.get_current_state()
print ""
## END_SUB_TUTORIAL
# Misc variables
self.box_name = ''
self.robot = robot
self.scene = scene
self.move_group = move_group
self.display_trajectory_publisher = display_trajectory_publisher
self.planning_frame = planning_frame
self.eef_link = eef_link
self.group_names = group_names
def cw3_example_script():
"""
This script will go through the main aspects of moveit and the components you will need to complete the coursework.
You can find more information on
"""
# Initialize moveit_commander and rospy node
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface')
# Initialize moveit scene interface (woeld surrounding the robot)
scene = moveit_commander.PlanningSceneInterface()
# Robot contains the entire state of the robot (iiw a and shadow hand)
global group, base_group, robot
robot = moveit_commander.RobotCommander()
group = moveit_commander.MoveGroupCommander('robot')
arm_group = moveit_commander.MoveGroupCommander('arm')
gripeer_group = moveit_commander.MoveGroupCommander('gripper')
base_group = moveit_commander.MoveGroupCommander('base')
# Create a DisplayTrajectory publisher which is used later to publish trajectories for RViz to visualize:
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
group.allow_replanning(True)
group.allow_looking(True)
# Set the number of planning attempts to find better solution
group.set_num_planning_attempts(10)
###Add obstacle
rospy.sleep(0.5)
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "odom"
box_pose.pose.position.x = 0
box_pose.pose.position.y = 3
box_pose.pose.position.z = 0.01
box_name = "wall"
scene.add_box(box_name, box_pose, size=(2, 0.01, 0.01))
rospy.sleep(0.5)
rospy.sleep(0.5)
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "odom"
box_pose.pose.position.x = -0.8
box_pose.pose.position.y = 3.5
box_pose.pose.position.z = 0.2
box_one = "box_1"
scene.add_box(box_one, box_pose, size=(0.4, 0.4, 0.4))
rospy.sleep(0.5)
rospy.sleep(0.5)
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "odom"
box_pose.pose.position.x = -0
box_pose.pose.position.y = 2.5
box_pose.pose.position.z = 0.2
box_two = "box_2"
scene.add_box(box_two, box_pose, size=(0.4, 0.4, 0.4))
rospy.sleep(0.5)
rospy.sleep(0.5)
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "odom"
box_pose.pose.position.x = 0.8
box_pose.pose.position.y = 3.5
box_pose.pose.position.z = 0.2
box_three = "box_3"
scene.add_box(box_three, box_pose, size=(0.4, 0.4, 0.4))
rospy.sleep(0.5)
#############################################################
#############################################################
resp = get_ik([2.6, 3, 0.3])
print len(resp.solution.joint_state.position)
# while(1):
# resp = get_ik([-1, 3, 2])
# if len(resp.solution.joint_state.position) != 0: break
# print len(resp.solution.joint_state.position), resp.solution.joint_state.name
#############################################################
print 'all finish'
# When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
pose.orientation.w = 0.0
return pose
if __name__ == '__main__':
mv.roscpp_initialize(["joint_states:=/panda/joint_states"])
rospy.wait_for_service(service_name)
rospy.init_node("main_py", log_level=rospy.DEBUG)
robot = mv.RobotCommander()
rospy.loginfo(robot.get_group_names())
rospy.loginfo(robot.get_link_names(
)) # gets all links in the model regardless of group membership
scene = mv.PlanningSceneInterface()
moveGroup = mv.MoveGroupCommander(move_group_name)
#joint_goal = moveGroup.get_current_joint_values()
#joint_goal[0] = 0
#joint_goal[1] = -pi/4
#joint_goal[2] = 0
#joint_goal[3] = -pi/2
#joint_goal[4] = 0
#joint_goal[5] = pi/3
#joint_goal[6] = 0
#moveGroup.go(joint_goal, wait = True)
#moveGroup.stop()
while not rospy.is_shutdown():
def main():
try:
print("creating basic interface")
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_and_pick', anonymous=True)
#creating a robot object
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
#Defining the group name
group_name = "panda_arm"
move_group = moveit_commander.MoveGroupCommander(group_name)
move_group.set_planning_time(45.0)
print "============ Printing initial robot state"
print robot.get_current_state()
################################################### Creating the environment: Namely a cube and 2 tables to pick from and place on.
#clean the scene
scene.remove_world_object("table1")
scene.remove_world_object("table2")
scene.remove_world_object("cube")
print"################Creating the scene"
dem = moveit_commander.PoseStamped()
dem.header.frame_id = "panda_link0"
#add a pickup table
dem.pose.position.x = 0.5
dem.pose.position.y = 0
dem.pose.position.z = 0.2
scene.add_box("table1", dem, (0.02, 0.02, 0.58))
#add a drop off table
dem.pose.position.x = 0
dem.pose.position.y = 0.5
dem.pose.position.z = 0.2
scene.add_box("table2", dem, (0.02, 0.02, 0.58))
#add an object to be grasped
dem.pose.position.x = 0.5
dem.pose.position.y = 0
dem.pose.position.z = 0.5
scene.add_box("cube", dem, (0.02, 0.02, 0.02))
rospy.sleep(1)
########################Resetting the robot arm to prevent singularity
print"Resetting: moving joints to defined orientation to avoid singularity"
joint_goal = move_group.get_current_joint_values()
joint_goal[0] = 0
joint_goal[1] = -pi/4
joint_goal[2] = 0
joint_goal[3] = -pi/2
joint_goal[4] = 0
joint_goal[5] = pi/3
joint_goal[6] = 0
move_group.go(joint_goal, wait=True)
move_group.stop()
rospy.sleep(1)
##################################################Approaching the cube and picking it up
print "################Moving to pick the cube"
grasps = Grasp()
#defining the grasp pose of the end effector
grasps.grasp_pose.header.frame_id = "panda_link0"
grasps.grasp_pose.pose.position.x = 0.415
grasps.grasp_pose.pose.position.y = 0
grasps.grasp_pose.pose.position.z = 0.5
quaternion = quaternion_from_euler(-pi/2, -pi/4, -pi/2)
grasps.grasp_pose.pose.orientation.x = quaternion[0]
grasps.grasp_pose.pose.orientation.y = quaternion[1]
grasps.grasp_pose.pose.orientation.z = quaternion[2]
grasps.grasp_pose.pose.orientation.w = quaternion[3]
#setting the pregrasp approach
grasps.pre_grasp_approach.direction.header.frame_id = "panda_link0"
grasps.pre_grasp_approach.direction.vector.x = 1.0
grasps.pre_grasp_approach.min_distance = 0.095
grasps.pre_grasp_approach.desired_distance = 0.115
#setting post grasp-retreat
grasps.post_grasp_retreat.direction.header.frame_id = "panda_link0"
grasps.post_grasp_retreat.direction.vector.z = 1.0
grasps.post_grasp_retreat.min_distance = 0.1
grasps.post_grasp_retreat.desired_distance = 0.25
#opening the hand
#add points
grasps.pre_grasp_posture.joint_names.append("panda_finger_joint1")
grasps.pre_grasp_posture.joint_names.append("panda_finger_joint2")
point1=JointTrajectoryPoint()
point1.positions.append(0.04)
point1.positions.append(0.04)
point1.time_from_start = rospy.Duration(0.5)
grasps.pre_grasp_posture.points.append(point1)
#closing the hand
#add points
grasps.grasp_posture.joint_names.append("panda_finger_joint1")
grasps.grasp_posture.joint_names.append("panda_finger_joint2")
point2=JointTrajectoryPoint()
point2.positions.append(0.00)
point2.positions.append(0.00)
point2.time_from_start = rospy.Duration(0.5)
grasps.grasp_posture.points.append(point2)
move_group.set_support_surface_name("table1")
move_group.pick("cube", grasps)
rospy.sleep(1.0)
print "============ Printing picked up robot state"
print robot.get_current_state()
##########################################################Moving the cube and placing it down
print"################Moving to place the cube"
placer = PlaceLocation()
#defining the grasp pose of the end effector
placer.place_pose.header.frame_id = "panda_link0"
placer.place_pose.pose.position.x = 0
placer.place_pose.pose.position.y = 0.5
placer.place_pose.pose.position.z = 0.5
quaternion = quaternion_from_euler(-pi, 0, pi/2)
placer.place_pose.pose.orientation.x = quaternion[0]
placer.place_pose.pose.orientation.y = quaternion[1]
placer.place_pose.pose.orientation.z = quaternion[2]
placer.place_pose.pose.orientation.w = quaternion[3]
#setting the pre-place approach
placer.pre_place_approach.direction.header.frame_id = "panda_link0"
placer.pre_place_approach.direction.vector.z = -1.0
placer.pre_place_approach.min_distance = 0.095
placer.pre_place_approach.desired_distance = 0.115
#setting post place-retreat
placer.post_place_retreat.direction.header.frame_id = "panda_link0"
placer.post_place_retreat.direction.vector.y = -1.0
placer.post_place_retreat.min_distance = 0.1
placer.post_place_retreat.desired_distance = 0.25
#opening the hand
#add points
placer.post_place_posture.joint_names.append("panda_finger_joint1")
placer.post_place_posture.joint_names.append("panda_finger_joint2")
point3 = JointTrajectoryPoint()
point3.positions.append(0.00)
point3.positions.append(0.00)
point3.time_from_start = rospy.Duration(0.5)
placer.post_place_posture.points.append(point3)
move_group.set_support_surface_name("table2")
move_group.place("cube", placer)
print "============ Printing placed/final robot state"
print robot.get_current_state()
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
def move_group_python_interface_tutorial():
## BEGIN_TUTORIAL
##
## Setup
## ^^^^^
## CALL_SUB_TUTORIAL imports
##
## First initialize moveit_commander and rospy.
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
group = moveit_commander.MoveGroupCommander("my_ur10_limited")
## We create this DisplayTrajectory publisher which is used below to publish
## trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory)
## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
print "============ Waiting for RVIZ..."
rospy.sleep(10)
print "============ Starting tutorial "
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
##
## We can get the name of the reference frame for this robot
print "============ Reference frame: %s" % group.get_planning_frame()
## We can also print the name of the end-effector link for this group
print "============ Reference frame: %s" % group.get_end_effector_link()
## We can get a list of all the groups in the robot
print "============ Robot Groups:"
print robot.get_group_names()
## Sometimes for debugging it is useful to print the entire state of the
## robot.
print "============ Printing robot state"
print robot.get_current_state()
print "============"
## Planning to a Pose goal
## ^^^^^^^^^^^^^^^^^^^^^^^
## We can plan a motion for this group to a desired pose for the
## end-effector
print "============ Generating plan 1"
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = 1.0
pose_target.position.x = 0.7
pose_target.position.y = -0.05
pose_target.position.z = 1.1
group.set_pose_target(pose_target)
## Now, we call the planner to compute the plan
## and visualize it if successful
## Note that we are just planning, not asking move_group
## to actually move the robot
plan1 = group.plan()
print "============ Waiting while RVIZ displays plan1..."
rospy.sleep(5)
## You can ask RVIZ to visualize a plan (aka trajectory) for you. But the
## group.plan() method does this automatically so this is not that useful
## here (it just displays the same trajectory again).
print "============ Visualizing plan1"
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan1)
display_trajectory_publisher.publish(display_trajectory)
print "============ Waiting while plan1 is visualized (again)..."
rospy.sleep(5)
## Moving to a pose goal
## ^^^^^^^^^^^^^^^^^^^^^
##
## Moving to a pose goal is similar to the step above
## except we now use the go() function. Note that
## the pose goal we had set earlier is still active
## and so the robot will try to move to that goal. We will
## not use that function in this tutorial since it is
## a blocking function and requires a controller to be active
## and report success on execution of a trajectory.
# Uncomment below line when working with a real robot
# group.go(wait=True)
## Planning to a joint-space goal
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
##
## Let's set a joint space goal and move towards it.
## First, we will clear the pose target we had just set.
group.clear_pose_targets()
## Then, we will get the current set of joint values for the group
group_variable_values = group.get_current_joint_values()
print "============ Joint values: ", group_variable_values
## Now, let's modify one of the joints, plan to the new joint
## space goal and visualize the plan
group_variable_values[0] = 1.0
group.set_joint_value_target(group_variable_values)
plan2 = group.plan()
print "============ Waiting while RVIZ displays plan2..."
rospy.sleep(5)
## Cartesian Paths
## ^^^^^^^^^^^^^^^
## You can plan a cartesian path directly by specifying a list of waypoints
## for the end-effector to go through.
waypoints = []
# start with the current pose
waypoints.append(group.get_current_pose().pose)
# first orient gripper and move forward (+x)
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 1.0
wpose.position.x = waypoints[0].position.x + 0.1
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z
waypoints.append(copy.deepcopy(wpose))
# second move down
wpose.position.z -= 0.10
waypoints.append(copy.deepcopy(wpose))
# third move to the side
wpose.position.y += 0.05
waypoints.append(copy.deepcopy(wpose))
## We want the cartesian path to be interpolated at a resolution of 1 cm
## which is why we will specify 0.01 as the eef_step in cartesian
## translation. We will specify the jump threshold as 0.0, effectively
## disabling it.
(plan3, fraction) = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print "============ Waiting while RVIZ displays plan3..."
rospy.sleep(5)
## Adding/Removing Objects and Attaching/Detaching Objects
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
## First, we will define the collision object message
collision_object = moveit_msgs.msg.CollisionObject()
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
## END_TUTORIAL
print "============ STOPPING"
def find_stuff():
# Always the first thing: Initialize node
rospy.init_node('position_finder_node', anonymous=True)
## First initialize moveit_commander and rospy.
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
## Instantiate a RobotCommander object. This object is an interface to the robot as a whole.
robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a MoveGroupCommander object. This object is an interface to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left arm.
group = moveit_commander.MoveGroupCommander("Arm")
## We create this DisplayTrajectory publisher which is used below to publish trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory)
## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
print "============ Waiting for RVIZ..."
rospy.sleep(2)
## Getting Basic Information
## We can get the name of the reference frame for this robot
print "============ Reference frame: %s" % group.get_planning_frame()
## We can also print the name of the end-effector link for this group
print "============ Reference frame: %s" % group.get_end_effector_link()
## We can get a list of all the groups in the robot
print "============ Robot Groups:"
print robot.get_group_names()
## Sometimes for debugging it is useful to print the entire state of the robot.
print "============ Printing robot state"
print robot.get_current_state()
print "============"
scene = moveit_commander.PlanningSceneInterface()
robot = moveit_commander.RobotCommander()
rospy.sleep(2)
# topic msgs: [ground_plane, jaco_on_table, cube0, cube1, ... , bucket] [String, Pose, Twist] <= ModelStates
rospy.Subscriber("/gazebo/model_states", ModelStates, get_positions)
for i in range(0, 2):
## Planning to a Pose goal - Go grab a cube
## We can plan a motion for this group to a desired pose for the end-effector
print "============ Generating plan 1"
#group.set_joint_value_target([1.57,0.,0.,0.])
## Let's setup the planner
group.set_planning_time(1.0)
group.set_goal_orientation_tolerance(1)
group.set_goal_tolerance(1)
group.set_goal_joint_tolerance(0.1)
group.set_num_planning_attempts(10)
pose_goal_1 = group.get_current_pose().pose
print '#', i, 'cube pose:', cube_pos.pose
#pose_goal_1.orientation = geometry_msgs.msg.Quaternion(*tf_conversions.transformations.quaternion_from_euler(0., -math.pi/2, 0.)) # cube orientation
pose_goal_1.orientation = cube_pos.pose.orientation
pose_goal_1.position = cube_pos.pose.position
#pose_goal_1.position.x = cube_pos.pose.position.x # cube global x position
#pose_goal_1.position.y = cube_pos.pose.position.y # cube global y position
#pose_goal_1.position.z = cube_pos.pose.position.z # cube global z position
print 'Cube pose goal:', pose_goal_1
group.set_pose_target(pose_goal_1)
## Now, we call the planner to compute the plan and visualize it if successful Note that we are just planning, not asking move_group to actually move the robot
#group.set_goal_position_tolerance(1.5)
plan_1 = group.plan()
print "============ Waiting while RVIZ displays plan 1..."
rospy.sleep(0.5)
## You can ask RVIZ to visualize a plan (aka trajectory) for you. But the group.plan() method does this automatically so this is not that useful here (it just displays the same trajectory again).
print "============ Visualizing plan 1"
display_trajectory_1 = moveit_msgs.msg.DisplayTrajectory()
display_trajectory_1.trajectory_start = robot.get_current_state()
display_trajectory_1.trajectory.append(plan_1)
display_trajectory_publisher.publish(display_trajectory_1)
print "============ Waiting while plan 1 is visualized (again)..."
rospy.sleep(0.5)
group.go(wait=True)
## second movement - Go to the bucket and leave the item
## ^^^^^^^^^^^^^^^^^^^^^
print "============ Generating plan 2"
pose_goal_2 = group.get_current_pose().pose
#pose_goal_2.orientation = geometry_msgs.msg.Quaternion(*tf_conversions.transformations.quaternion_from_euler(0., -math.pi/2, 0.)) # cube orientation
pose_goal_2.orientation = bucket_pos.pose.orientation
pose_goal_2.position.x = bucket_pos.pose.position.x # cube global x position
pose_goal_2.position.y = bucket_pos.pose.position.y # cube global y position
pose_goal_2.position.z = bucket_pos.pose.position.z # cube global z position
#print 'Bucket pose goal:', pose_goal_2
group.set_pose_target(pose_goal_2)
plan_2 = group.plan()
rospy.sleep(0.5)
print "============ Visualizing plan 2"
display_trajectory_2 = moveit_msgs.msg.DisplayTrajectory()
display_trajectory_2.trajectory_start = robot.get_current_state()
display_trajectory_2.trajectory.append(plan_2)
display_trajectory_publisher.publish(display_trajectory_2)
print "============ Waiting while plan 2 is visualized (again)..."
rospy.sleep(0.5)
group.go(wait=True)
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
R = rospy.Rate(10)
while not rospy.is_shutdown():
R.sleep()
def main():
try:
#rospy.init_node('avalos_limb_py')
#moveit_commander.roscpp_initialize(sys.argv)
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial',anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = 'right_arm'
group = moveit_commander.MoveGroupCommander(group_name)
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# We can get the name of the reference frame for this robot:
planning_frame = group.get_planning_frame()
#frecuency for Sawyer robot
f=100
rate = rospy.Rate(f)
# add gripper
if(True):
gripper = intera_interface.Gripper('right_gripper')
gripper.calibrate()
gripper.open()
moveit_robot_state = RobotState()
joint_state = JointState()
joint_state.header = Header()
joint_state.header.stamp = rospy.Time.now()
joint_state.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6']
#Define topic
pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10)
# Class to record
#data=Data()
#Class limb to acces information sawyer
limb = Limb()
limb.move_to_neutral()
neutral=[-7.343481724930712e-07,-1.1799709303615113,2.7170121530417646e-05,2.1799982536216014,-0.00023687847544184848,0.5696772114967752,3.1411912264073045]
base=[0.4045263671875, 0.3757021484375, -2.31678515625, 1.4790908203125, 2.14242578125, 2.1983291015625, 0.8213740234375]
s6_down=[0.1123427734375, 0.398875, -2.4554755859375, 0.4891044921875, 2.4769873046875, 1.575130859375, 1.531140625]
s6_up=[0.1613271484375, 0.3916650390625, -2.441814453125, 0.6957587890625, 2.515578125, 1.679708984375, 1.459033203125]
obj_up=[-0.74389453125, 0.153580078125, -1.7190927734375, 0.7447021484375, 1.72510546875, 1.5934130859375, 0.317576171875]
obj_down=[-0.7055927734375, 0.5030830078125, -1.7808125, 0.7994287109375, 2.0973154296875, 1.35018359375, 0.259451171875]
centro=[0.0751162109375, 0.1868447265625, -1.93045703125, 1.425337890625, 1.7726181640625, 1.904037109375, 0.4765615234375]
#points_1=path(neutral,centro,5)
#points_2=path(centro,objeto,5)
#points=path_full([neutral,objeto,centro,s6,centro,neutral],[5,5,5,5,5])
#points=path_full([neutral,obj_up,obj_down,obj_up,centro,s6,centro,neutral],[3,3,3,3,3,3,3])
alfa=0.5
p1=path_full([neutral,obj_up,obj_down],[2,3,2])
p2=path_full([obj_down,obj_up,centro,base,s6_up,s6_down],[3,3,3,3,3,2])
p3=path_full([s6_down,base,centro,obj_up],[3,3])
opt_1=Opt_avalos(p1,f,alfa)
opt_2=Opt_avalos(p2,f,alfa)
opt_3=Opt_avalos(p3,f,alfa)
v_time1=opt_1.full_time()
v_time2=opt_2.full_time()
v_time3=opt_3.full_time()
j,v,a,jk=generate_path_cub(p1,v_time1,f)
ext=len(j[0,:])
#Build message
my_msg=JointCommand()
# POSITION_MODE
my_msg.mode=4
my_msg.names=["right_j0","right_j1","right_j2","right_j3","right_j4","right_j5","right_j6"]
for n in xrange(ext):
my_msg.position=[j[0][n],j[1][n],j[2][n],j[3][n],j[4][n],j[5][n],j[6][n]]
my_msg.velocity=[v[0][n],v[1][n],v[2][n],v[3][n],v[4][n],v[5][n],v[6][n]]
my_msg.acceleration=[a[0][n],a[1][n],a[2][n],a[3][n],a[4][n],a[5][n],a[6][n]]
pub.publish(my_msg)
rate.sleep()
gripper.close()
time.sleep(1)
j,v,a,jk=generate_path_cub(p2,v_time2,f)
ext=len(j[0,:])
#Build message
my_msg=JointCommand()
# POSITION_MODE
my_msg.mode=4
my_msg.names=["right_j0","right_j1","right_j2","right_j3","right_j4","right_j5","right_j6"]
for n in xrange(ext):
my_msg.position=[j[0][n],j[1][n],j[2][n],j[3][n],j[4][n],j[5][n],j[6][n]]
my_msg.velocity=[v[0][n],v[1][n],v[2][n],v[3][n],v[4][n],v[5][n],v[6][n]]
my_msg.acceleration=[a[0][n],a[1][n],a[2][n],a[3][n],a[4][n],a[5][n],a[6][n]]
pub.publish(my_msg)
rate.sleep()
gripper.open()
j,v,a,jk=generate_path_cub(p3,v_time3,f)
ext=len(j[0,:])
#Build message
my_msg=JointCommand()
# POSITION_MODE
my_msg.mode=4
my_msg.names=["right_j0","right_j1","right_j2","right_j3","right_j4","right_j5","right_j6"]
for n in xrange(ext):
my_msg.position=[j[0][n],j[1][n],j[2][n],j[3][n],j[4][n],j[5][n],j[6][n]]
my_msg.velocity=[v[0][n],v[1][n],v[2][n],v[3][n],v[4][n],v[5][n],v[6][n]]
my_msg.acceleration=[a[0][n],a[1][n],a[2][n],a[3][n],a[4][n],a[5][n],a[6][n]]
pub.publish(my_msg)
rate.sleep()
'''
group.clear_pose_targets()
group.set_start_state_to_current_state()s
# We can get the joint values from the group and adjust some of the values:
pose_goal = geometry_msgs.msg.Pose()
# Orientation
pose_goal.orientation.x = -1
pose_goal.orientation.y = 0.0
pose_goal.orientation.z = 0.0
pose_goal.orientation.w = 0.0
q0=np.array([])
q1=np.array([])
q2=np.array([])
q3=np.array([])
q4=np.array([])
q5=np.array([])
q6=np.array([])
# Cartesian position
pose_goal.position.x = -0.1
pose_goal.position.y = 0.35
pose_goal.position.z = 0.05
pose_goal=Pose(Point(-0.1,0.6,0.05),Quaternion(-1,0,0,0))
group.set_pose_target(pose_goal)
a=group.plan()
points=a.joint_trajectory.points
n=len(points)
joint_state.position = [points[n-1].positions[0], points[n-1].positions[1], points[n-1].positions[2], points[n-1].positions[3],points[n-1].positions[4], points[n-1].positions[5], points[n-1].positions[6]]
moveit_robot_state.joint_state = joint_state
group.set_start_state(moveit_robot_state)
for i in range(n):
q0=np.append(q0, points[i].positions[0])
q1=np.append(q1, points[i].positions[1])
q2=np.append(q2, points[i].positions[2])
q3=np.append(q3, points[i].positions[3])
q4=np.append(q4, points[i].positions[4])
q5=np.append(q5, points[i].positions[5])
q6=np.append(q6, points[i].positions[6])
print "q000",q0
# Cartesian position
pose_goal.position.x = 0.43
pose_goal.position.y = -0.4
pose_goal.position.z = 0.2
group.set_pose_target(pose_goal)
a=group.plan()
points=a.joint_trajectory.points
n=len(points)
joint_state.position = [points[n-1].positions[0], points[n-1].positions[1], points[n-1].positions[2], points[n-1].positions[3],points[n-1].positions[4], points[n-1].positions[5], points[n-1].positions[6]]
moveit_robot_state.joint_state = joint_state
group.set_start_state(moveit_robot_state)
for i in range(n-1): # Si se repite un numero en posicion entra en un bug
q0=np.append(q0, points[i+1].positions[0])
q1=np.append(q1, points[i+1].positions[1])
q2=np.append(q2, points[i+1].positions[2])
q3=np.append(q3, points[i+1].positions[3])
q4=np.append(q4, points[i+1].positions[4])
q5=np.append(q5, points[i+1].positions[5])
q6=np.append(q6, points[i+1].positions[6])
q=np.array([q0,q1,q2,q3,q4,q5,q6])
print "q001",q0
print q[0]
#return 0
alfa=0.5
start = time.time()
opt=Opt_avalos(q,f,alfa)
v_time=opt.full_time()
j,v,a,jk=generate_path_cub(q,v_time,f)
ext=len(j[0,:])
end = time.time()
print('Process Time:', end-start)
print ext
#save_matrix(j,"data_p.txt",f)
#save_matrix(v,"data_v.txt",f)
#save_matrix(a,"data_a.txt",f)
#save_matrix(jk,"data_y.txt",f)
v_jk=sqrt(np.mean(np.square(jk)))
print("Opt Time:",v_time)
print("Min Time:",m_time)
#print('Optimizacion:',opt.result())
print('Costo Tiempo:',len(j[0])/float(100.0))
print('Costo Jerk:', v_jk)
# Position init
#raw_input('Iniciar_CT_initial_position?')
#limb.move_to_joint_positions({"right_j6": ik1[6],"right_j5": ik1[5], "right_j4": ik1[4], "right_j3": ik1[3], "right_j2":
#ik1[2],"right_j1": ik1[1],"right_j0": ik1[0]})
raw_input('Iniciar_CT_execute?')
#Build message
my_msg=JointCommand()
# POSITION_MODE
my_msg.mode=4
my_msg.names=["right_j0","right_j1","right_j2","right_j3","right_j4","right_j5","right_j6"]
#data.writeon(str(alfa)+"trayectoria.txt")
print("Control por trayectoria iniciado.")
#time.sleep(0.25)
for n in xrange(ext):
my_msg.position=[j[0][n],j[1][n],j[2][n],j[3][n],j[4][n],j[5][n],j[6][n]]
my_msg.velocity=[v[0][n],v[1][n],v[2][n],v[3][n],v[4][n],v[5][n],v[6][n]]
my_msg.acceleration=[a[0][n],a[1][n],a[2][n],a[3][n],a[4][n],a[5][n],a[6][n]]
#pub.publish(my_msg)
rate.sleep()
print("Control por trayectoria terminado.")
time.sleep(0.25)
#data.writeoff()
'''
print("Programa terminado.")
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user.')
def move_pick_cube():
print("============ Starting setup ============")
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_pick_cube', anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("Arm")
end_effector_link = group.get_end_effector_link()
# trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory)
print("============ Reference frame: %s" % group.get_planning_frame())
print("============ End effector frame: %s" %
group.get_end_effector_link())
print("============ Robot Groups:")
print(robot.get_group_names())
print("============ Printing robot state")
print(robot.get_current_state())
print("============")
print("============put cubes and bucket into moveit workspace")
# If we're coming from another script we might want to remove the objects
for i in xrange(0, 6):
if "cube{}".format(i) in scene.get_known_object_names():
scene.remove_world_object("cube{}".format(i))
if "bucket" in scene.get_known_object_names():
scene.remove_world_object("bucket")
##put cubes and bucket into moveit workspace
##load param into a list
cube_bucket_orient = rospy.get_param("cube_bucket_orientRPY")
bucket_pose = rospy.get_param("bucket_XYZ")
cube_num = rospy.get_param("cube_num")
table_height = 0.75 #table height=0.8
orient_quat = geometry_msgs.msg.Quaternion(
*tf_conversions.transformations.quaternion_from_euler(
cube_bucket_orient[0], cube_bucket_orient[1],
cube_bucket_orient[2]))
cube_pose_list = list()
for i in xrange(0, cube_num):
if rospy.has_param("cube{}_XYZ".format(i)):
cube_pose_list.append(rospy.get_param("cube{}_XYZ".format(i)))
cube_pose_list[i][2] = table_height
##prepare the cubes info
cube_info = geometry_msgs.msg.PoseStamped()
cube_info.header.frame_id = "robot_base"
for i in xrange(0, cube_num):
cube_info.pose.position.x = cube_pose_list[i][0]
cube_info.pose.position.y = cube_pose_list[i][1]
cube_info.pose.position.z = table_height #table height
cube_info.pose.orientation = orient_quat
print("cube{} x:{}, y:{}".format(i, cube_pose_list[i][0],
cube_pose_list[i][1]))
scene.add_box("cube{}".format(i), cube_info,
(0.05, 0.05, 0.05)) ##found in cubic.urdf
#prepare the bucket info
bucket_info = geometry_msgs.msg.PoseStamped()
bucket_info.header.frame_id = "robot_base"
bucket_info.pose.position.x = bucket_pose[0]
bucket_info.pose.position.y = bucket_pose[1]
bucket_info.pose.position.z = table_height
bucket_info.pose.orientation = orient_quat
scene.add_box("bucket", bucket_info,
(0.2, 0.2, 0.2)) ##cannot found in bucket.dae
print("============moveit workspace loading finished")
## setup the planner parameters
group.set_goal_orientation_tolerance(0.01)
group.set_goal_tolerance(0.01)
group.set_goal_joint_tolerance(0.01)
group.set_num_planning_attempts(100)
pub = rospy.Publisher("/jaco/joint_control", JointState, queue_size=1)
#################make hand towards table
print("hand towards table")
hand_pick_quat = geometry_msgs.msg.Quaternion(0.347709304721,
-0.646715871525,
0.298597553716,
0.609669026475)
pose_goal = group.get_current_pose(end_effector_link).pose
ini_pose = pose_goal
pose_goal.orientation = hand_pick_quat
group.set_start_state_to_current_state()
group.set_pose_target(pose_goal, end_effector_link)
plan1_1 = group.plan()
group.execute(plan1_1)
base_x = 0.5788
base_y = -0.0154
dis_threshold_far = 0.6
dis_threshold_near = 0.26
for i_cube_check in xrange(0, cube_num):
dis = np.sqrt(
np.square(cube_pose_list[i_cube_check][0] - base_x) +
np.square(cube_pose_list[i_cube_check][1] - base_y))
if dis > dis_threshold_far:
print(
'cube{} is at a dangerous position(too far) in a distance:{} to the robot base, the task of picking cube{} might fail'
.format(i_cube_check, dis, i_cube_check))
elif dis < dis_threshold_near:
print(
'cube{} is at a dangerous position(too near) in a distance:{} to the robot base, the task of picking cube{} might fail'
.format(i_cube_check, dis, i_cube_check))
else:
print(
'cube{} is at a reachable position in a distance:{} to the robot base'
.format(i_cube_check, dis))
for i_cube in xrange(0, cube_num):
######################### middle point
print("cube{} go to middle point".format(i_cube))
pose_goal.position.x = 0.30
pose_goal.position.y = 0.0
group.set_start_state_to_current_state()
group.set_pose_target(pose_goal, end_effector_link)
plan1_2 = group.plan()
group.execute(plan1_2)
rospy.sleep(1.)
########################### go to the upper of cube
print("cube{} go to the upper of cube".format(i_cube))
waypoints12 = list()
pose_goal12 = group.get_current_pose(end_effector_link).pose
ini_pose12 = pose_goal12
waypoints12.append(copy.deepcopy(pose_goal12))
pose_goal12.position.x = cube_pose_list[i_cube][0]
pose_goal12.position.y = cube_pose_list[i_cube][1]
pose_goal12.position.z = table_height + 0.2
waypoints12.append(copy.deepcopy(pose_goal12))
# plan and execute
fraction = 0.0
attempts_max = 100
attempts = 0
while fraction < 0.6 and attempts < attempts_max:
(plan12,
fraction) = group.compute_cartesian_path(waypoints12, 0.01, 0.0)
attempts += 1
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
## Moving to a pose goal
if fraction > 0.4: #fraction == 1.0:
rospy.loginfo("Path planning " + str(fraction) +
" success after " + str(attempts_max) + " attempts.")
rospy.loginfo("Path computed successfully. Moving the arm.")
group.execute(plan12)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(attempts_max) + " attempts.")
group.set_start_state_to_current_state()
group.set_pose_target(pose_goal12, end_effector_link)
plan12 = group.plan()
group.execute(plan12)
rospy.sleep(1.)
########################go down to the cube
print("cube{} go down to the cube".format(i_cube))
waypoints2 = list()
pose_goal2 = group.get_current_pose(end_effector_link).pose
ini_pose2 = pose_goal2
waypoints2.append(copy.deepcopy(pose_goal2))
pose_goal2.position.x = cube_pose_list[i_cube][0]
pose_goal2.position.y = cube_pose_list[i_cube][1]
pose_goal2.position.z = table_height + 0.17
waypoints2.append(copy.deepcopy(pose_goal2))
fraction = 0.0
attempts_max = 100
attempts = 0
while fraction < 0.8 and attempts < attempts_max:
(plan2,
fraction) = group.compute_cartesian_path(waypoints2, 0.01, 0.0)
attempts += 1
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
## Moving to a pose goal
if fraction > 0.8: #fraction == 1.0:
rospy.loginfo("Path planning " + str(fraction) +
" success after " + str(attempts_max) + " attempts.")
rospy.loginfo("Path computed successfully. Moving the arm.")
group.execute(plan2)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(attempts_max) + " attempts.")
continue #the action of going down to the cube needs to be done perfectly, if not, quit
rospy.sleep(1.)
##############################################################
################################################close the hand
print("cube{} close hand".format(i_cube))
currentJointState = rospy.wait_for_message("/joint_states", JointState)
print('Received!')
currentJointState.header.stamp = rospy.get_rostime()
tmp = 0.7
# tmp_tuple=tuple([tmp] + list(currentJointState.position[1:]))
currentJointState.position = tuple(
list(currentJointState.position[:6]) + [tmp] + [tmp] + [tmp])
rate = rospy.Rate(10) # 10hz
for i in range(3):
pub.publish(currentJointState)
print('Published!')
rate.sleep()
print('end!')
rospy.sleep(5.)
#############################################
#############################################hand up
print("cube{} hand up from cube".format(i_cube))
waypoints3 = list()
pose_goal3 = group.get_current_pose(end_effector_link).pose
ini_pose3 = copy.deepcopy(pose_goal3)
#print(pose_goal3)
waypoints3.append(copy.deepcopy(pose_goal3))
pose_goal3.position.z = ini_pose3.position.z + 0.2 #0.4
waypoints3.append(copy.deepcopy(pose_goal3))
fraction = 0.0
attempts_max = 100
attempts = 0
while fraction < 1.0 and attempts < attempts_max:
(plan3,
fraction) = group.compute_cartesian_path(waypoints3, 0.01, 0.0)
attempts += 1
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
## Moving to a pose goal
if fraction > 0.4: #fraction == 1.0:
rospy.loginfo("Path planning " + str(fraction) +
" success after " + str(attempts_max) + " attempts.")
rospy.loginfo("Path computed successfully. Moving the arm.")
group.execute(plan3)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(attempts_max) + " attempts.")
pose_goal3.position.z = ini_pose3.position.z + 0.2 # 0.3#have to use the x,y of current pose instead of cube's pose when going up
group.set_start_state_to_current_state()
group.set_pose_target(pose_goal3, end_effector_link)
plan3 = group.plan()
group.execute(plan3)
rospy.sleep(1.)
rospy.sleep(1.)
####################################################################
###########################################go to the upper of bucket
print("cube{} go to the upper of bucket".format(i_cube))
waypoints34 = list()
pose_goal34 = group.get_current_pose(end_effector_link).pose
ini_pose34 = pose_goal34
waypoints34.append(copy.deepcopy(pose_goal34))
pose_goal34.position.x = bucket_pose[0]
pose_goal34.position.y = bucket_pose[1]
pose_goal34.position.z = table_height + 0.6
pose_goal34.orientation = hand_pick_quat
waypoints34.append(copy.deepcopy(pose_goal34))
(plan34,
fraction) = group.compute_cartesian_path(waypoints34, 0.01, 0.0)
## Moving to a pose goal
group.execute(plan34, wait=True)
rospy.sleep(1.)
############################################go to bucket
waypoints4 = list()
pose_goal4 = group.get_current_pose(end_effector_link).pose
ini_pose4 = pose_goal4
waypoints4.append(copy.deepcopy(pose_goal4))
pose_goal4.position.x = bucket_pose[0]
pose_goal4.position.y = bucket_pose[1]
pose_goal4.position.z = table_height + 0.5
pose_goal4.orientation = hand_pick_quat
#print(pose_goal4)
waypoints4.append(copy.deepcopy(pose_goal4))
(plan4, fraction) = group.compute_cartesian_path(waypoints4, 0.01, 0.0)
## Moving to a pose goal
group.execute(plan4, wait=True)
rospy.sleep(1.)
#################################open the hand
print("cube{} open hand".format(cube_num))
currentJointState = rospy.wait_for_message("/joint_states", JointState)
print('Received!')
currentJointState.header.stamp = rospy.get_rostime()
tmp = 0.005
# tmp_tuple=tuple([tmp] + list(currentJointState.position[1:]))
currentJointState.position = tuple(
list(currentJointState.position[:6]) + [tmp] + [tmp] + [tmp])
rate = rospy.Rate(10) # 10hz
for i in range(3):
pub.publish(currentJointState)
print('Published!')
rate.sleep()
print('end!')
rospy.sleep(2.)
#############################################
#############################################hand up
print("cube{} hand up from bucket".format(i_cube))
waypoints5 = list()
pose_goal5 = group.get_current_pose(end_effector_link).pose
ini_pose5 = pose_goal5
waypoints5.append(pose_goal5)
pose_goal5.position.z = table_height + 0.5 # have to use the x,y of current pose instead of bucket's pose when going up
pose_goal5.orientation = hand_pick_quat
waypoints5.append(pose_goal5)
pose_goal5.position.z = table_height + 0.6
waypoints5.append(pose_goal5)
(plan5, fraction) = group.compute_cartesian_path(waypoints5, 0.01, 0.0)
## Moving to a pose goal
group.execute(plan5, wait=True)
rospy.sleep(1.)
print("go back")
waypoints6 = list()
pose_goal6 = group.get_current_pose(end_effector_link).pose
waypoints6.append(pose_goal6)
pose_goal6.position = ini_pose.position
waypoints6.append(pose_goal6)
(plan6, fraction) = group.compute_cartesian_path(waypoints6, 0.01, 0.0)
## Moving to a pose goal
group.execute(plan6, wait=True)
rospy.sleep(1.)
# END_TUTORIAL
print("============ STOPPING")
R = rospy.Rate(10)
while not rospy.is_shutdown():
R.sleep()
def __init__(self):
self.scene = moveit_commander.PlanningSceneInterface()
def __init__(self):
# The current status of the joints.
self.JointState = JointTrajectoryControllerState()
# The servo power's status of the robot.
self.ServoPowerState = Bool()
# The fault power's status of the robot.
self.PowerFaultState = Bool()
# The reference coordinate in the calculations of the elfin_basic_api node
self.RefCoordinate = String()
# The end coordinate in the calculations of the elfin_basic_api node
self.EndCoordinate = String()
#The value of the dynamic parameters of elfin_basic_api, e.g. velocity scaling.
self.DynamicArgs = Config()
# get the reference coordinate name of elfin_basic_api from the response of this service.
self.call_ref_coordinate = rospy.ServiceProxy('elfin_basic_api/get_reference_link', SetBool)
self.call_ref_coordinate_req = SetBoolRequest()
# get the current position of elfin_ros_control from the response of this service.
self.call_current_position = rospy.ServiceProxy('elfin_ros_control/elfin/get_current_position', SetBool)
self.call_current_position_req = SetBoolRequest()
# call service recognize_position of elfin_ros_control.
self.call_recognize_position = rospy.ServiceProxy('elfin_ros_control/elfin/recognize_position', SetBool)
self.call_recognize_position_req = SetBoolRequest()
self.call_recognize_position_req.data = True
# get the end coordinate name of elfin_basic_api from the response of this service.
self.call_end_coordinate = rospy.ServiceProxy('elfin_basic_api/get_end_link', SetBool)
self.call_end_coordinate_req = SetBoolRequest()
# for publishing joint goals to elfin_basic_api
self.JointsPub = rospy.Publisher('elfin_basic_api/joint_goal', JointState, queue_size=1)
self.JointsGoal = JointState()
# for publishing cart goals to elfin_basic_api
self.CartGoalPub = rospy.Publisher('elfin_basic_api/cart_goal', PoseStamped, queue_size=1)
self.CartPos = PoseStamped()
# for pub cart path
self.CartPathPub = rospy.Publisher('elfin_basic_api/cart_path_goal', PoseArray, queue_size=1)
self.CartPath = PoseArray()
self.CartPath.header.stamp=rospy.get_rostime()
self.CartPath.header.frame_id='elfin_base_link'
# for pub one specific joint action to elfin_teleop_joint_cmd_no_limit
self.JointCmdPub = rospy.Publisher('elfin_teleop_joint_cmd_no_limit', Int64 , queue_size=1)
self.JointCmd = Int64()
# for pub multi specific joint action to elfin_teleop_joint_cmd_no_limit
self.JointsCmdPub = rospy.Publisher('changyuan_joints_cmd', JointsFloat64, queue_size=1)
self.JointsCmd = JointsFloat64()
# action client, send goal to move_group
self.action_client = SimpleActionClient('elfin_module_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
self.action_goal = FollowJointTrajectoryGoal()
#self.goal_list = JointTrajectoryPoint()
self.goal_list = []
self.joints_ = []
self.ps_ = []
self.listener = tf.TransformListener()
self.robot=moveit_commander.RobotCommander()
self.scene=moveit_commander.PlanningSceneInterface()
self.group=moveit_commander.MoveGroupCommander('elfin_arm')
self.ref_link_name=self.group.get_planning_frame()
self.end_link_name=self.group.get_end_effector_link()
self.ref_link_lock=threading.Lock()
self.end_link_lock=threading.Lock()
self.call_teleop_stop=rospy.ServiceProxy('elfin_basic_api/stop_teleop', SetBool)
self.call_teleop_stop_req=SetBoolRequest()
self.call_teleop_joint=rospy.ServiceProxy('elfin_basic_api/joint_teleop',SetInt16)
self.call_teleop_joint_req=SetInt16Request()
self.call_teleop_joints=rospy.ServiceProxy('elfin_basic_api/joints_teleops',SetFloat64s)
self.call_teleop_joints_req=SetFloat64sRequest()
self.call_teleop_cart=rospy.ServiceProxy('elfin_basic_api/cart_teleop', SetInt16)
self.call_teleop_cart_req=SetInt16Request()
self.call_move_homing=rospy.ServiceProxy('elfin_basic_api/home_teleop', SetBool)
self.call_move_homing_req=SetBoolRequest()
self.call_reset=rospy.ServiceProxy(self.elfin_driver_ns+'clear_fault', SetBool)
self.call_reset_req=SetBoolRequest()
self.call_reset_req.data=True
self.call_power_on = rospy.ServiceProxy(self.elfin_driver_ns+'enable_robot', SetBool)
self.call_power_on_req=SetBoolRequest()
self.call_power_on_req.data=True
self.call_power_off = rospy.ServiceProxy(self.elfin_driver_ns+'disable_robot', SetBool)
self.call_power_off_req = SetBoolRequest()
self.call_power_off_req.data=True
self.call_velocity_setting = rospy.ServiceProxy('elfin_basic_api/set_velocity_scale', SetFloat64)
self.call_velocity_req = SetFloat64Request()
self._velocity_scale = 0.78
self.set_velocity_scale(self._velocity_scale)
pass
def test():
## First initialize moveit_commander and rospy.
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('test', anonymous=True)
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
rospy.sleep(2)
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
rightarm_group = moveit_commander.MoveGroupCommander("right_arm")
pose_copy = rightarm_group.get_current_pose().pose
rightarm_pose_target = geometry_msgs.msg.Pose()
rightarm_pose_target.orientation.w = 0
rightarm_pose_target.orientation.x = 1
rightarm_pose_target.orientation.y = 0
rightarm_pose_target.orientation.z = 0
rightarm_pose_target.position.x = 0.06
rightarm_pose_target.position.y = 0.21
rightarm_pose_target.position.z = 0.75
rightarm_group.set_joint_value_target(rightarm_pose_target, "Link23", True)
first_plan = rightarm_group.plan()
print "-----------------------------------size of first_plan"
print len(first_plan.joint_trajectory.points)
if first_plan.joint_trajectory.points:
print "success"
# rightarm_group.execute(first_plan)
listener = tf.TransformListener()
find = False
while not find:
try:
(trans,
rot) = listener.lookupTransform('/base_link', '/Link24',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
print trans
find = True
x = trans[0]
y = trans[1]
z = 1 + trans[2]
leftarm_group = moveit_commander.MoveGroupCommander("left_arm")
#to get the trans in link21 and value in link24
link21x = 0.068898
link21y = 0.067365
link21z = 0.895772
link24x = 0.068870
link24y = 0.063403
link24z = 0.706849
size0 = max(x, link21x, link24x) - min(
x, link21x,
link24x) + 0.04 #[0.04, 0.04, 0.05] is the size of link
size1 = max(y, link21y, link24y) - min(y, link21y, link24y) + 0.04
size2 = max(z, link21z, link24z) - min(z, link21z, link24z) + 0.05
size = (size0, size1, size2)
#set the collision to avoid the leftarm plan to touch the rightarm when they are move Simultaneously
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = leftarm_group.get_planning_frame()
box_pose.pose.orientation.w = 1
box_pose.pose.position.x = x
box_pose.pose.position.y = y
box_pose.pose.position.z = z
scene.add_box("box", box_pose, size)
leftarm_pose_target = geometry_msgs.msg.Pose()
leftarm_pose_target.orientation.w = 0
leftarm_pose_target.orientation.x = 1
leftarm_pose_target.orientation.y = 0
leftarm_pose_target.orientation.z = 0
leftarm_pose_target.position.x = -0.06
leftarm_pose_target.position.y = 0.21
leftarm_pose_target.position.z = 0.75
leftarm_group.set_joint_value_target(leftarm_pose_target, "Link13",
True)
second_plan = leftarm_group.plan()
print "-----------------------------------size of second_plan"
print len(second_plan.joint_trajectory.points)
scene.remove_world_object("box")
if second_plan.joint_trajectory.points:
print "second success"
leftarm_group.execute(second_plan)
else:
rightarm_group.set_joint_value_target(pose_copy, "Link23",
True)
recovery_plan = rightarm_group.plan()
rightarm_group.execute(recovery_plan)
def __init__(self):
### MoveIt!
moveit_commander.roscpp_initialize(sys.argv)
br = tf.TransformBroadcaster()
#rospy.init_node('move_group_planner',
# anonymous=True)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.plan_scene = moveit_commander.PlanningScene()
self.group_1st = moveit_commander.MoveGroupCommander("panda_left")
self.group_2nd = moveit_commander.MoveGroupCommander("panda_right")
self.group_3rd = moveit_commander.MoveGroupCommander("panda_top")
self.group_list = [self.group_1st, self.group_2nd, self.group_3rd]
self.group_chain = moveit_commander.MoveGroupCommander(
"panda_closed_chain")
self.group_chairup = moveit_commander.MoveGroupCommander(
"panda_chair_up")
self.hand_left = moveit_commander.MoveGroupCommander("hand_left")
self.hand_right = moveit_commander.MoveGroupCommander("hand_right")
self.hand_top = moveit_commander.MoveGroupCommander("hand_top")
self.hand_list = [self.hand_left, self.hand_right, self.hand_top]
self.display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# We can get the name of the reference frame for this robot:
self.planning_frame = self.group_1st.get_planning_frame()
print("============ Reference frame: %s" % self.planning_frame)
# We can also print the name of the end-effector link for this group:
self.eef_link = self.group_1st.get_end_effector_link()
print("============ End effector: %s" % self.eef_link)
# We can get the name of the reference frame for this robot:
self.planning_frame = self.group_3rd.get_planning_frame()
print("============ Reference frame: %s" % self.planning_frame)
# We can also print the name of the end-effector link for this group:
self.eef_link = self.group_3rd.get_end_effector_link()
print("============ End effector: %s" % self.eef_link)
# We can get a list of all the groups in the robot:
self.group_names = self.robot.get_group_names()
print("============ Robot Groups:", self.robot.get_group_names())
rospy.sleep(1)
self.stefan = SceneObject()
# self.scene.remove_attached_object(self.group_1st.get_end_effector_link())
# self.scene.remove_attached_object(self.group_3rd.get_end_effector_link())
# self.scene.remove_world_object()
### Franka Collision
self.set_collision_behavior = rospy.ServiceProxy(
'franka_control/set_force_torque_collision_behavior',
franka_control.srv.SetForceTorqueCollisionBehavior)
#self.set_collision_behavior.wait_for_service()
self.active_controllers = []
self.listener = tf.TransformListener()
self.tr = TransformerROS()
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "base"
box_pose.pose.orientation.w = 1.0
box_pose.pose.position.x = 0.8
box_pose.pose.position.z = 0.675
box_pose2 = geometry_msgs.msg.PoseStamped()
box_pose2.header.frame_id = "base"
box_pose2.pose.orientation.w = 1.0
box_pose2.pose.position.x = 0.8
box_pose2.pose.position.y = -0.25
box_pose2.pose.position.z = 1.0
self.scene.add_box("box", box_pose, size=(1.2, 0.5, 0.15))
def __init__(self):
'''
Constructor of the MoveitObjectHandler class.
'''
moveit_commander.roscpp_initialize(sys.argv)
self.scene = moveit_commander.PlanningSceneInterface()
def move_group_interface():
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_interface', anonymous=True)
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
dual_arm_group = moveit_commander.MoveGroupCommander("dual_arm")
# left_arm_group = moveit_commander.MoveGroupCommander("left_arm")
# print "============ Reference frame: %s" % left_arm_group.get_planning_frame()
# print "============End Effector: %s" % left_arm_group.get_end_effector_link()
# left_gripper_group = moveit_commander.MoveGroupCommander("left_gripper")
# right_arm_group = moveit_commander.MoveGroupCommander("right_arm")
## We create this DisplayTrajectory publisher which is used below to publish
## trajectories for RVIZ to visualize.
#display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=10)
dual_arm_group.set_named_target("both_down")
plan1 = dual_arm_group.plan()
rospy.sleep(0.5)
dual_arm_group.go()
print "...Home..."
rospy.sleep(1)
dual_arm_group.set_named_target("both_grab3")
plan1 = dual_arm_group.plan()
rospy.sleep(0.5)
dual_arm_group.go()
print "...Waiting to lift..."
rospy.sleep(20)
dual_arm_group.set_named_target("both_lift")
plan1 = dual_arm_group.plan()
rospy.sleep(0.5)
dual_arm_group.go()
print "...Lifted..."
rospy.sleep(1)
# right_arm_group.set_named_target("right_grab")
# plan2= right_arm_group.plan()
# rospy.sleep(1)
# left_arm_group.go()
# right_arm_group.go()
# print "...1st Pose Done..."
# rospy.sleep(1)
# grippers_group = moveit_commander.MoveGroupCommander("grippers")
# for i in range(2):
# grippers_group.set_named_target("both_open")
# plan2 = grippers_group.plan()
# rospy.sleep(1)
# grippers_group.go()
# print "...Both Open..."
# rospy.sleep(2)
# grippers_group.set_named_target("both_close")
# plan2 = grippers_group.plan()
# rospy.sleep(1)
# grippers_group.go()
# print "...Both Close..."
# rospy.sleep(2)
# roll = 1.57
# pitch = 0
# yaw = 0
# q = quaternion_from_euler(roll, pitch, yaw)
# goal.position.x = 0.555;
# goal.position.y = 0.146;
# goal.position.z = 1.156;
# goal.orientation.w = 0.991428;
# goal.orientation.x = 0.0085588;
# goal.orientation.y = -0.087665;
# goal.orientation.z = -0.0964952;
# left_arm_group.set_named_target("random valid")
# plan1= left_arm_group.plan()
# rospy.sleep(6)
# left_arm_group.go(wait=True)
# position:
# x: 0.0250186168009
# y: 0.487010565504
# z: 0.470970706717
# orientation:
# x: -0.521156274313
# y: -0.398770697363
# z: -0.287207590558
# w: 0.69777494122
# pose_target = geometry_msgs.msg.Pose()
# pose_target.position.x = 0.0250186168009
# pose_target.position.y = 0.487010565504
# pose_target.position.z = 0.470970706717
# pose_target.orientation.w = 0.69777494122
# pose_target.orientation.x =-0.521156274313
# pose_target.orientation.y = -0.398770697363
# pose_target.orientation.z =-0.287207590558
# left_arm_group.set_pose_target(pose_target)
# plan1 = left_arm_group.plan()
# rospy.sleep(6)
# left_arm_group.go(wait=True)
print "...Done..."
moveit_commander.roscpp_shutdown()
def __init__(self):
super(roboticprintingcontrol, self).__init__()
# First initialize 'moveit_commander' and a 'rospy' node:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('robotic_printing_control', anonymous=True)
# Instantiate a "RobotCommander" object. Provides information such as the robot's
# kinematic model and the robot's current joint states, this object is outer-level
# robot
robot = moveit_commander.RobotCommander()
# Instantiate a "PlanningSceneInterface". This provides a remote interface
# for getting, setting, and updating the robot's internal understanding of
# the surrounding world:
scene = moveit_commander.PlanningSceneInterface()
# Instantiate a "MoveGroupCommander". This object is an interface to a planning
# group (group of joints). In this project, the robot that we will use is panda
# therefore, set the group's name to be "panda_arm". If in some certain cases,
# we are using a different robot, change this value to the name of your robot
# arm planning group.
# This interface can be used to plan and execute motions:
group_name = "panda_arm"
move_group = moveit_commander.MoveGroupCommander(group_name)
# Create a "DisplayTrajectory" ROS publisher which is used to visualise
# trajectories in RVIZ:
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
""" --------------------- Getting Basic Information -------------------------- """
"""
One thing worth noting is that in my system, the version of python is
2.7, therefore, when print things, ignore parenthesis, otherwise, do
not forget to add them back
"""
# Get the name of the reference frame for panda robot
planning_frame = move_group.get_planning_frame()
print "============ Planning frame: %s" % planning_frame
# Print the name of the end-effector link for this group, in this project
# the end effector is supposed to be a 3D printer head although it has not
# been completely designed yet
end_effector = move_group.get_end_effector_link()
print "============ End effector link: %s" % end_effector
# Get a list of all the groups in the robot:
group_names = robot.get_group_names()
print "============ Available Planning Groups:", robot.get_group_names(
)
# For debugging purposes, print the entire state of the
# robot:
print "============ Printing robot state"
print robot.get_current_state()
# Misc variables
self.box_name = ''
self.robot = robot
self.scene = scene
self.move_group = move_group
self.display_trajectory_publisher = display_trajectory_publisher
self.planning_frame = planning_frame
self.eef_link = end_effector
self.group_names = group_names
def move_group_python_interface_tutorial():
# BEGIN_TUTORIAL
# First initialize moveit_commander and rospy.
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial',
anonymous=True)
scene = moveit_commander.PlanningSceneInterface()
robot = moveit_commander.RobotCommander()
# Instantiate a MoveGroupCommander object. This object is an interface
# to one group of joints. In this case the group is the joints in the left
# arm. This interface can be used to plan and execute motions on the left
# arm.
group = moveit_commander.MoveGroupCommander("arm")
# We create this DisplayTrajectory publisher which is used below to publish
# trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory)
# Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
# print "============ Waiting for RVIZ..."
# rospy.sleep(2)
# print "============ Starting tutorial "
# Getting Basic Information
# ^^^^^^^^^^^^^^^^^^^^^^^^^
##
# We can get the name of the reference frame for this robot
print "============ Reference frame: %s" % group.get_planning_frame()
# We can also print the name of the end-effector link for this group
print "============ Reference frame: %s" % group.get_end_effector_link()
# We can get a list of all the groups in the robot
print "============ Robot Groups:"
print robot.get_group_names()
# Sometimes for debugging it is useful to print the entire state of the
# robot.
print "============ Printing robot state"
print robot.get_current_state()
print "============"
# rospy.sleep(2)
print "============ Generating plan "
group.set_joint_value_target([-0.5, -0.5, 0.0, 0.0])
# Let's setup the planner
group.set_planning_time(2.0)
group.set_goal_orientation_tolerance(1)
group.set_goal_tolerance(1)
group.set_goal_joint_tolerance(0.1)
group.set_num_planning_attempts(20)
# Now, we call the planner to compute the plan
# and visualize it if successful
# Note that we are just planning, not asking move_group
# to actually move the robot
# group.set_goal_position_tolerance(1.5)
plan1 = group.plan()
# print "============ Waiting while RVIZ displays plan1..."
# rospy.sleep(0.5)
# You can ask RVIZ to visualize a plan (aka trajectory) for you. But the
# group.plan() method does this automatically so this is not that useful
# here (it just displays the same trajectory again).
# print "============ Visualizing plan1"
#display_trajectory = moveit_msgs.msg.DisplayTrajectory()
#display_trajectory.trajectory_start = robot.get_current_state()
# display_trajectory.trajectory.append(plan1)
# display_trajectory_publisher.publish(display_trajectory);
# print "============ Waiting while plan1 is visualized (again)..."
# rospy.sleep(0.5)
# Moving to a pose goal
# ^^^^^^^^^^^^^^^^^^^^^
group.go(wait=True)
group.set_joint_value_target([0.0, 0.0, 0.0, 0.0])
plan1 = group.plan()
group.go(wait=True)
group.set_joint_value_target([-0.5, -0.5, 0.0, 0.0])
plan1 = group.plan()
group.go(wait=True)
group.set_joint_value_target([0.0, 0.0, 0.0, 0.0])
plan1 = group.plan()
group.go(wait=True)
# When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
# END_TUTORIAL
print "============ STOPPING"
R = rospy.Rate(10)
while not rospy.is_shutdown():
R.sleep()
def robotiq_85_moveit_test():
## BEGIN_TUTORIAL
##
## Setup
## ^^^^^
## CALL_SUB_TUTORIAL imports
##
## First initialize moveit_commander and rospy.
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('robotiq_85_moveit_test', anonymous=True)
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
group = moveit_commander.MoveGroupCommander("gripper")
## We create this DisplayTrajectory publisher which is used below to publish
## trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=10)
rospy.sleep(2)
scene.remove_world_object("floor")
# publish a demo scene
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.0
p.pose.position.y = 0.0
p.pose.position.z = -0.01
p.pose.orientation.w = 1.0
scene.add_box("floor", p, (2.0, 2.0, 0.02))
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
##
## We can get the name of the reference frame for this robot
print "============ Reference frame: %s" % group.get_planning_frame()
## We can also print the name of the end-effector link for this group
print "============ Reference frame: %s" % group.get_end_effector_link()
## We can get a list of all the groups in the robot
print "============ Robot Groups:"
print robot.get_group_names()
## Sometimes for debugging it is useful to print the entire state of the
## robot.
print "============ Printing robot state"
print robot.get_current_state()
print "============"
group.set_planner_id("RRTConnectkConfigDefault")
## Planning to a Pose goal
## ^^^^^^^^^^^^^^^^^^^^^^^
## We can plan a motion for this group to a desired pose for the
## end-effector
print "============ Generating plan 1"
x = 0
while x < 100 and not rospy.is_shutdown():
group.set_random_target()
## Now, we call the planner to compute the plan
## and visualize it if successful
## Note that we are just planning, not asking move_group
## to actually move the robot
plan1 = group.plan()
group.go(wait=True)
x += 1
print "============ Waiting while RVIZ displays plan1..."
rospy.sleep(5)
## You can ask RVIZ to visualize a plan (aka trajectory) for you. But the
## group.plan() method does this automatically so this is not that useful
## here (it just displays the same trajectory again).
print "============ Visualizing plan1"
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan1)
display_trajectory_publisher.publish(display_trajectory)
print "============ Waiting while plan1 is visualized (again)..."
rospy.sleep(5)
## Moving to a pose goal
## ^^^^^^^^^^^^^^^^^^^^^
##
## Moving to a pose goal is similar to the step above
## except we now use the go() function. Note that
## the pose goal we had set earlier is still active
## and so the robot will try to move to that goal. We will
## not use that function in this tutorial since it is
## a blocking function and requires a controller to be active
## and report success on execution of a trajectory.
# Uncomment below line when working with a real robot
group.go(wait=True)
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
## END_TUTORIAL
print "============ STOPPING"
def handle_req(req):
## First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
## Instantiate a `RobotCommander`_ object. Provides information such as the robot's
## kinematic model and the robot's current pose states
robot = moveit_commander.RobotCommander()
## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface
## for getting, setting, and updating the robot's internal understanding of the
## surrounding world:
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a `MoveGroupCommander`_ object. This object is an interface
## to a planning group (group of poses).
## This interface can be used to plan and execute motions:
group_name = req.group
## print "============ Robot Groups:"
print robot.get_group_names()
## print "============ Printing robot state"
print robot.get_current_state()
move_group = moveit_commander.MoveGroupCommander(group_name.data)
print "Pose is: "
print move_group.get_current_pose().pose
print "Orientation is: "
print req.orientation
print "x is: "
print req.posex
print "y is: "
print req.posey
print "z is: "
print req.posez
## Create a `DisplayTrajectory`_ ROS publisher which is used to display
## trajectories in Rviz:
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# We can get the pose values from the group and adjust some of the values:
print "============ Generating plan.."
pose_goal = geometry_msgs.msg.Pose()
print pose_goal
# plan a motion for this group
pose_goal.orientation.w = req.orientation.data
pose_goal.position.x = req.posex.data
pose_goal.position.y = req.posey.data
pose_goal.position.z = req.posez.data
move_group.set_pose_target(pose_goal)
# The go command can be called with pose values, poses, or without any
# parameters if you have already set the pose or pose target for the group
move_group.go(pose_goal, wait=True)
# Calling ``stop()`` ensures that there is no residual movement
move_group.stop()
# It is always good to clear your targets after planning with poses.
move_group.clear_pose_targets()
rep = inverse_service()
rep.message = "This action was planned at %s" %rospy.get_time()
rep.res=True
return rep
def __init__(self,parent,id):
the_size=(700, 570)
wx.Frame.__init__(self,parent,id,'Elfin Control Panel',pos=(250,100))
self.panel=wx.Panel(self)
font=self.panel.GetFont()
font.SetPixelSize((12, 24))
self.panel.SetFont(font)
self.listener = tf.TransformListener()
self.robot=moveit_commander.RobotCommander()
self.scene=moveit_commander.PlanningSceneInterface()
self.group=moveit_commander.MoveGroupCommander('elfin_arm')
self.controller_ns='elfin_arm_controller/'
self.elfin_driver_ns='elfin_ros_control/elfin/'
self.elfin_basic_api_ns='elfin_basic_api/'
self.joint_names=rospy.get_param(self.controller_ns+'joints', [])
self.ref_link_name=self.group.get_planning_frame()
self.end_link_name=self.group.get_end_effector_link()
self.ref_link_lock=threading.Lock()
self.end_link_lock=threading.Lock()
self.js_display=[0]*6 # joint_states
self.jm_button=[0]*6 # joints_minus
self.jp_button=[0]*6 # joints_plus
self.js_label=[0]*6 # joint_states
self.ps_display=[0]*6 # pcs_states
self.pm_button=[0]*6 # pcs_minus
self.pp_button=[0]*6 # pcs_plus
self.ps_label=[0]*6 # pcs_states
self.display_init()
self.key=[]
btn_height=390
btn_lengths=[]
self.power_on_btn=wx.Button(self.panel, label=' Servo On ', name='Servo On',
pos=(20, btn_height))
btn_lengths.append(self.power_on_btn.GetSize()[0])
btn_total_length=btn_lengths[0]
self.power_off_btn=wx.Button(self.panel, label=' Servo Off ', name='Servo Off')
btn_lengths.append(self.power_off_btn.GetSize()[0])
btn_total_length+=btn_lengths[1]
self.reset_btn=wx.Button(self.panel, label=' Clear Fault ', name='Clear Fault')
btn_lengths.append(self.reset_btn.GetSize()[0])
btn_total_length+=btn_lengths[2]
self.home_btn=wx.Button(self.panel, label='Home', name='home_btn')
btn_lengths.append(self.home_btn.GetSize()[0])
btn_total_length+=btn_lengths[3]
self.stop_btn=wx.Button(self.panel, label='Stop', name='Stop')
btn_lengths.append(self.stop_btn.GetSize()[0])
btn_total_length+=btn_lengths[4]
btn_interstice=(550-btn_total_length)/4
btn_pos_tmp=btn_lengths[0]+btn_interstice+20
self.power_off_btn.SetPosition((btn_pos_tmp, btn_height))
btn_pos_tmp+=btn_lengths[1]+btn_interstice
self.reset_btn.SetPosition((btn_pos_tmp, btn_height))
btn_pos_tmp+=btn_lengths[2]+btn_interstice
self.home_btn.SetPosition((btn_pos_tmp, btn_height))
btn_pos_tmp+=btn_lengths[3]+btn_interstice
self.stop_btn.SetPosition((btn_pos_tmp, btn_height))
self.servo_state_label=wx.StaticText(self.panel, label='Servo state:',
pos=(590, btn_height-10))
self.servo_state_show=wx.TextCtrl(self.panel, style=(wx.TE_CENTER |wx.TE_READONLY),
value='', pos=(600, btn_height+10))
self.servo_state=bool()
self.fault_state_label=wx.StaticText(self.panel, label='Fault state:',
pos=(590, btn_height+60))
self.fault_state_show=wx.TextCtrl(self.panel, style=(wx.TE_CENTER |wx.TE_READONLY),
value='', pos=(600, btn_height+80))
self.fault_state=bool()
self.reply_show_label=wx.StaticText(self.panel, label='Result:',
pos=(20, btn_height+120))
self.reply_show=wx.TextCtrl(self.panel, style=(wx.TE_CENTER |wx.TE_READONLY),
value='', size=(670, 30), pos=(20, btn_height+140))
link_textctrl_length=(btn_pos_tmp-40)/2
self.ref_links_show_label=wx.StaticText(self.panel, label='Ref. link:',
pos=(20, btn_height+60))
self.ref_link_show=wx.TextCtrl(self.panel, style=(wx.TE_READONLY),
value=self.ref_link_name, size=(link_textctrl_length, 30),
pos=(20, btn_height+80))
self.end_link_show_label=wx.StaticText(self.panel, label='End link:',
pos=(link_textctrl_length+30, btn_height+60))
self.end_link_show=wx.TextCtrl(self.panel, style=(wx.TE_READONLY),
value=self.end_link_name, size=(link_textctrl_length, 30),
pos=(link_textctrl_length+30, btn_height+80))
self.set_links_btn=wx.Button(self.panel, label='Set links', name='Set links')
self.set_links_btn.SetPosition((btn_pos_tmp, btn_height+75))
# the variables about velocity scaling
velocity_scaling_init=rospy.get_param(self.elfin_basic_api_ns+'velocity_scaling',
default=0.4)
default_velocity_scaling=str(round(velocity_scaling_init, 2))
self.velocity_setting_label=wx.StaticText(self.panel, label='Velocity Scaling',
pos=(20, btn_height-70))
self.velocity_setting=wx.Slider(self.panel, value=int(velocity_scaling_init*100),
minValue=1, maxValue=100,
style = wx.SL_HORIZONTAL,
size=(500, 30),
pos=(45, btn_height-50))
self.velocity_setting_txt_lower=wx.StaticText(self.panel, label='1%',
pos=(20, btn_height-45))
self.velocity_setting_txt_upper=wx.StaticText(self.panel, label='100%',
pos=(550, btn_height-45))
self.velocity_setting_show=wx.TextCtrl(self.panel,
style=(wx.TE_CENTER|wx.TE_READONLY),
value=default_velocity_scaling,
pos=(600, btn_height-55))
self.velocity_setting.Bind(wx.EVT_SLIDER, self.velocity_setting_cb)
self.teleop_api_dynamic_reconfig_client=dynamic_reconfigure.client.Client(self.elfin_basic_api_ns,
config_callback=self.basic_api_reconfigure_cb)
self.dlg=wx.Dialog(self.panel, title='messag')
self.dlg.Bind(wx.EVT_CLOSE, self.closewindow)
self.dlg_panel=wx.Panel(self.dlg)
self.dlg_label=wx.StaticText(self.dlg_panel, label='hello', pos=(15, 15))
self.set_links_dlg=wx.Dialog(self.panel, title='Set links', size=(400, 100))
self.set_links_dlg_panel=wx.Panel(self.set_links_dlg)
self.sld_ref_link_show=wx.TextCtrl(self.set_links_dlg_panel, style=wx.TE_PROCESS_ENTER,
value='', pos=(20, 20), size=(link_textctrl_length, 30))
self.sld_end_link_show=wx.TextCtrl(self.set_links_dlg_panel, style=wx.TE_PROCESS_ENTER,
value='', pos=(20, 70), size=(link_textctrl_length, 30))
self.sld_set_ref_link_btn=wx.Button(self.set_links_dlg_panel, label='Update ref. link',
name='Update ref. link')
self.sld_set_ref_link_btn.SetPosition((link_textctrl_length+30, 15))
self.sld_set_end_link_btn=wx.Button(self.set_links_dlg_panel, label='Update end link',
name='Update end link')
self.sld_set_end_link_btn.SetPosition((link_textctrl_length+30, 65))
self.set_links_dlg.SetSize((link_textctrl_length+self.sld_set_ref_link_btn.GetSize()[0]+50, 120))
self.call_teleop_joint=rospy.ServiceProxy(self.elfin_basic_api_ns+'joint_teleop',
SetInt16)
self.call_teleop_joint_req=SetInt16Request()
self.call_teleop_cart=rospy.ServiceProxy(self.elfin_basic_api_ns+'cart_teleop',
SetInt16)
self.call_teleop_cart_req=SetInt16Request()
self.call_teleop_stop=rospy.ServiceProxy(self.elfin_basic_api_ns+'stop_teleop',
SetBool)
self.call_teleop_stop_req=SetBoolRequest()
self.call_stop=rospy.ServiceProxy(self.elfin_basic_api_ns+'stop_teleop',
SetBool)
self.call_stop_req=SetBoolRequest()
self.call_stop_req.data=True
self.stop_btn.Bind(wx.EVT_BUTTON,
lambda evt, cl=self.call_stop,
rq=self.call_stop_req :
self.call_set_bool_common(evt, cl, rq))
self.call_reset=rospy.ServiceProxy(self.elfin_driver_ns+'clear_fault', SetBool)
self.call_reset_req=SetBoolRequest()
self.call_reset_req.data=True
self.reset_btn.Bind(wx.EVT_BUTTON,
lambda evt, cl=self.call_reset,
rq=self.call_reset_req :
self.call_set_bool_common(evt, cl, rq))
self.call_power_on=rospy.ServiceProxy(self.elfin_driver_ns+'enable_robot', SetBool)
self.call_power_on_req=SetBoolRequest()
self.call_power_on_req.data=True
self.power_on_btn.Bind(wx.EVT_BUTTON,
lambda evt, cl=self.call_power_on,
rq=self.call_power_on_req :
self.call_set_bool_common(evt, cl, rq))
self.call_power_off=rospy.ServiceProxy(self.elfin_driver_ns+'disable_robot', SetBool)
self.call_power_off_req=SetBoolRequest()
self.call_power_off_req.data=True
self.power_off_btn.Bind(wx.EVT_BUTTON,
lambda evt, cl=self.call_power_off,
rq=self.call_power_off_req :
self.call_set_bool_common(evt, cl, rq))
self.call_move_homing=rospy.ServiceProxy(self.elfin_basic_api_ns+'home_teleop',
SetBool)
self.call_move_homing_req=SetBoolRequest()
self.call_move_homing_req.data=True
self.home_btn.Bind(wx.EVT_LEFT_DOWN,
lambda evt, cl=self.call_move_homing,
rq=self.call_move_homing_req :
self.call_set_bool_common(evt, cl, rq))
self.home_btn.Bind(wx.EVT_LEFT_UP,
lambda evt, mark=100:
self.release_button(evt, mark) )
self.call_set_ref_link=rospy.ServiceProxy(self.elfin_basic_api_ns+'set_reference_link', SetString)
self.call_set_end_link=rospy.ServiceProxy(self.elfin_basic_api_ns+'set_end_link', SetString)
self.set_links_btn.Bind(wx.EVT_BUTTON, self.show_set_links_dialog)
self.sld_set_ref_link_btn.Bind(wx.EVT_BUTTON, self.update_ref_link)
self.sld_set_end_link_btn.Bind(wx.EVT_BUTTON, self.update_end_link)
self.sld_ref_link_show.Bind(wx.EVT_TEXT_ENTER, self.update_ref_link)
self.sld_end_link_show.Bind(wx.EVT_TEXT_ENTER, self.update_end_link)
self.action_client=SimpleActionClient(self.controller_ns+'follow_joint_trajectory',
FollowJointTrajectoryAction)
self.action_goal=FollowJointTrajectoryGoal()
self.action_goal.trajectory.joint_names=self.joint_names
self.SetMinSize(the_size)
self.SetMaxSize(the_size)
def __init__(self):
print
"Motion Planning Initializing..."
# Prepare the mutex for synchronization
self.mutex = Lock()
# Some info and conventions about the robot that we hard-code in here
# min and max joint values are not read in Python urdf, so we must hard-code them here
self.num_joints = 7
self.q_min = []
self.q_max = []
self.q_min.append(-3.1459);
self.q_max.append(3.1459)
self.q_min.append(-3.1459);
self.q_max.append(3.1459)
self.q_min.append(-3.1459);
self.q_max.append(3.1459)
self.q_min.append(-3.1459);
self.q_max.append(3.1459)
self.q_min.append(-3.1459);
self.q_max.append(3.1459)
self.q_min.append(-3.1459);
self.q_max.append(3.1459)
self.q_min.append(-3.1459);
self.q_max.append(3.1459)
# How finely to sample each joint
self.q_sample = [0.05, 0.05, 0.05, 0.1, 0.1, 0.1, 0.1]
self.joint_names = ["lwr_arm_0_joint",
"lwr_arm_1_joint",
"lwr_arm_2_joint",
"lwr_arm_3_joint",
"lwr_arm_4_joint",
"lwr_arm_5_joint",
"lwr_arm_6_joint"]
# Subscribes to information about what the current joint values are.
rospy.Subscriber("/joint_states", sensor_msgs.msg.JointState,
self.joint_states_callback)
# Subscribe to command for motion planning goal
rospy.Subscriber("/motion_planning_goal", geometry_msgs.msg.Transform,
self.move_arm_cb)
# Publish trajectory command
self.pub_trajectory = rospy.Publisher("/joint_trajectory", trajectory_msgs.msg.JointTrajectory,
queue_size=1)
# Initialize variables
self.joint_state = sensor_msgs.msg.JointState()
# Wait for moveit IK service
rospy.wait_for_service("compute_ik")
self.ik_service = rospy.ServiceProxy('compute_ik', moveit_msgs.srv.GetPositionIK)
print
"IK service ready"
# Wait for validity check service
rospy.wait_for_service("check_state_validity")
self.state_valid_service = rospy.ServiceProxy('check_state_validity',
moveit_msgs.srv.GetStateValidity)
print
"State validity service ready"
# Initialize MoveIt
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group_name = "lwr_arm"
self.group = moveit_commander.MoveGroupCommander(self.group_name)
print
"MoveIt! interface ready"
# Options
self.subsample_trajectory = True
print
"Initialization done."
def main():
# Grasp service
grasp_service_client = GraspServiceClient()
# Moveit and node setup
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node("moveit_commander")
atexit.register(moveit_commander.roscpp_shutdown)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
# TF setup
## tf2
tf_buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tf_buffer)
br = tf.TransformBroadcaster()
# Getting point cloud
cloud_info = grasp_service_client.get_cloud_centroid()
centroid = np.array(cloud_info['cloud_centroid'])
print "Got solution: ", cloud_info
print centroid
target_pos = centroid + np.array([-0.095,-0.01,0.07])
target_q = Quaternion()
target_q.set_from_euler([0,np.pi/2,0])
target_transform = Transform3(p=target_pos,q=target_q)
rate = rospy.Rate(30)
# while not rospy.is_shutdown():
# br.sendTransform(target_transform.p,
# target_transform.q,
# rospy.Time.now(),
# "grasp_goal",
# "world")
# rate.sleep()
br.sendTransform(target_transform.p,
target_transform.q,
rospy.Time.now(),
"grasp_goal",
"world")
########################################################
arm = moveit_commander.MoveGroupCommander("left_hand_arm")
arm.set_max_velocity_scaling_factor(0.3)
arm.set_max_acceleration_scaling_factor(0.3)
arm_initial_pose = arm.get_current_pose().pose
arm_initial_joints = arm.get_current_joint_values()
# Pre-grasp
target_pose = transform2PoseMsg(target_transform)
arm.set_pose_target(target_pose,end_effector_link="left_hand_palm_link")
arm.go(wait=True)
rospy.sleep(2)
arm.clear_pose_targets()
arm.stop()
# Grasp
target_pos = centroid + np.array([-0.095,-0.01,0.03])
target_q = Quaternion()
target_q.set_from_euler([0,np.pi/2,0])
target_transform = Transform3(p=target_pos,q=target_q)
target_pose = transform2PoseMsg(target_transform)
arm.set_pose_target(target_pose,end_effector_link="left_hand_palm_link")
arm.go(wait=True)
rospy.sleep(2)
arm.clear_pose_targets()
arm.stop()
gripper = moveit_commander.MoveGroupCommander("left_hand")
gripper_joint_values = gripper.get_current_joint_values()
print "Left hand: ", gripper_joint_values
# ### Open
gripper_joint_values[0] = 0.0
gripper.set_joint_value_target(gripper_joint_values)
gripper.go(wait=True)
# ### Close
gripper_joint_values[0] = 0.75
gripper.set_joint_value_target(gripper_joint_values)
gripper.go(wait=True)
# Move up
target_pos = centroid + np.array([-0.095,-0.01,0.12])
target_q = Quaternion()
target_q.set_from_euler([0,np.pi/2,0])
target_transform = Transform3(p=target_pos,q=target_q)
target_pose = transform2PoseMsg(target_transform)
arm.set_pose_target(target_pose,end_effector_link="left_hand_palm_link")
arm.go(wait=True)
rospy.sleep(2)
arm.clear_pose_targets()
arm.stop()
def move_group_python_interface_tutorial():
## First initialize moveit_commander and rospy.
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_commander', anonymous=True)
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
group = moveit_commander.MoveGroupCommander("manipulator")
## We create this DisplayTrajectory publisher which is used below to publish
## trajectories for RVIZ to visualize.
#display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory,queue_size=20)
## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
#print "============ Waiting for RVIZ..."
rospy.sleep(10)
#print "============ Starting tutorial "
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
##
## We can get the name of the reference frame for this robot
#group.set_planning_frame() = 'base_link'
print "============ Reference frame: %s" % group.get_planning_frame()
## We can also print the name of the end-effector link for this group
print "============ Reference frame: %s" % group.get_end_effector_link()
## We can get a list of all the groups in the robot
print "============ Robot Groups:"
print robot.get_group_names()
## Sometimes for debugging it is useful to print the entire state of the
## robot.
print "============ Printing robot state"
print robot.get_current_state()
print "============"
#set_io = on
## Planning to a Pose goal
## ^^^^^^^^^^^^^^^^^^^^^^^
## We can plan a motion for this group to a desired pose for the
## end-effector
# print "============ Generating plan 1"
#listener = tf.TransformListener()
# # while not rospy.is_shutdown():
# try:
# listener.waitForTransform('object_frame', 'base_link',rospy.Time.now(), rospy.Duration(4.0))
# (trans,rot) = listener.lookupTransform('object_frame', 'base_link', rospy.Time.now())
# print "trans"
# print trans
# print "rot"
# print rot
# except (tf.LookupException, tf.ConnectivityException):
# print "123"
pose_target = geometry_msgs.msg.Pose()
pose_target.position.x = 0.01
pose_target.position.y = 0.23
pose_target.position.z = 0.69
pose_target.orientation.x = 0.6
pose_target.orientation.y = 0.76
pose_target.orientation.z = -0.43
pose_target.orientation.w = 0.46
# #Eigen::Affine3d pose = Eigen::Translation3d(0.028, 0.793, 0.390)
# # * Eigen::Quaterniond(-0.014, 0.733, 0.680, -0.010);
# #group.setPoseTarget(pose);
group.set_pose_target(pose_target)
#group.set_pose_target(object_pose)
#group.set_named_target("up")
#plan1 = group.get_random_joint_values()
## Now, we call the planner to compute the plan
## and visualize it if successful
## Note that we are just planning, not asking move_group
## to actually move the robot
group.set_planner_id("RRTConnectkConfigDefault")
group.set_planning_time(50)
plan1 = group.plan()
group.execute(plan1)
#print "============ Waiting while RVIZ displays plan1..."
#rospy.sleep(5)
# display_trajectory = moveit_msgs.msg.DisplayTrajectory()
# display_trajectory.trajectory_start = robot.get_current_state()
# display_trajectory.trajectory.append(plan1)
# display_trajectory_publisher.publish(display_trajectory);
#print "============ Waiting while plan1 is visualized (again)..."
#rospy.sleep(5)
# Uncomment below line when working with a real robot
#group.go(wait=True)
# Use execute instead if you would like the robot to follow
# the plan that has already been computed
#group.execute(plan1)
#set_io = on
#group.clear_pose_targets()
## Then, we will get the current set of joint values for the group
#joint_angles = group.get_current_joint_values()
#print "============ Joint values: ", joint_angles
## Now, let's modify one of the joints, plan to the new joint
## space goal and visualize the plan
#joint_angles = [0.0,0.0,0.0,0.0,0.0,0.0]
#joint_angles[0] = 1.0
#group.set_joint_value_target(joint_angles)
#plan2 = group.plan()
#display_trajectory.trajectory.append(plan2)
#display_trajectory_publisher.publish(display_trajectory);
#group.execute(plan2)
joint_angles = group.get_current_joint_values()
#print "============ Waiting while RVIZ displays plan2..."
#rospy.sleep(5)
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
print "============ STOPPING"
def __init__(self):
super(MoveGroupPythonInteface, self).__init__()
## BEGIN_SUB_TUTORIAL setup
##
## First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface', anonymous=True)
## Instantiate a `RobotCommander`_ object. This object is the outer-level interface to
## the robot:
robot = moveit_commander.RobotCommander()
## Instantiate a `PlanningSceneInterface`_ object. This object is an interface
## to the world surrounding the robot:
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a `MoveGroupCommander`_ object. This object is an interface
## to one group of joints. In this case the group is the joints in the Panda
## arm so we set ``group_name = panda_arm``. If you are using a different robot,
## you should change this value to the name of your robot arm planning group.
## This interface can be used to plan and execute motions on the Panda:
group = moveit_commander.MoveGroupCommander("arm")
gripper_group = moveit_commander.MoveGroupCommander("gripper")
## We create a `DisplayTrajectory`_ publisher which is used later to publish
## trajectories for RViz to visualize:
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
## END_SUB_TUTORIAL
## BEGIN_SUB_TUTORIAL basic_info
##
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
# We can get the name of the reference frame for this robot:
planning_frame = group.get_planning_frame()
print "============ Reference frame: %s" % planning_frame
# We can also print the name of the end-effector link for this group:
eef_link = group.get_end_effector_link()
print "============ End effector: %s" % eef_link
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print "============ Robot Groups:", robot.get_group_names()
# Sometimes for debugging it is useful to print the entire state of the
# robot:
# print "============ Printing robot state"
# print robot.get_current_state()
# print ""
## END_SUB_TUTORIAL
# Misc variables
self.box_name = ''
self.robot = robot
self.scene = scene
self.group = group
self.display_trajectory_publisher = display_trajectory_publisher
self.planning_frame = planning_frame
self.eef_link = eef_link
self.group_names = group_names
def igtl_importer(self):
global pub_igtl_transform_out
pub_igtl_transform_out = rospy.Publisher('IGTL_TRANSFORM_OUT',
igtltransform,
queue_size=10)
# self.pub_Marker = rospy.Publisher('marker_entry', MarkerArray, queue_size=10)
self.pub_entryMarker = rospy.Publisher('marker_entry',
Marker,
queue_size=1)
self.pub_targetMarker = rospy.Publisher('marker_target',
Marker,
queue_size=1)
self.pub_cortexMarker = rospy.Publisher('marker_cortex',
Marker,
queue_size=1)
# In ROS, nodes are uniquely named. If two nodes with the same
# node are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('igtl_importer', anonymous=True)
rospy.Subscriber("IGTL_TRANSFORM_IN", igtltransform, self.cb_transform)
rospy.Subscriber("IGTL_POINT_IN", igtlpoint, self.cb_point)
rospy.Subscriber("IGTL_STRING_IN", igtlstring, self.cb_string)
## BEGIN_SUB_TUTORIAL setup
##
## First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
# rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
## Instantiate a `RobotCommander`_ object. Provides information such as the robot's
## kinematic model and the robot's current joint states
robot = moveit_commander.RobotCommander()
## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface
## for getting, setting, and updating the robot's internal understanding of the
## surrounding world:
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a `MoveGroupCommander`_ object. This object is an interface
## to a planning group (group of joints). In this tutorial the group is the primary
## arm joints in the Panda robot, so we set the group's name to "panda_arm".
## If you are using a different robot, change this value to the name of your robot
## arm planning group.
## This interface can be used to plan and execute motions:
group_name = "ur5"
move_group = moveit_commander.MoveGroupCommander(group_name)
## Create a `DisplayTrajectory`_ ROS publisher which is used to display
## trajectories in Rviz:
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# Misc variables
# self.box_name = ''
self.robot = robot
self.scene = scene
self.move_group = move_group
self.display_trajectory_publisher = display_trajectory_publisher
self.entry = geometry_msgs.msg.Pose()
self.target = geometry_msgs.msg.Pose()
self.validEntry = False
self.validTarget = False
self.validMarkers = False
self.markerArray = MarkerArray()
# self.planning_frame = planning_frame
# self.eef_link = eef_link
# self.group_names = group_names
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
# parsing input arguments
parser = argparse.ArgumentParser(description='Redundancy Resolution')
parser.add_argument('-q','--q_angle_change', default=-90.0,
metavar='[degrees]', type=float,help='desired joint angle change in degrees')
parser.add_argument('-i','--index', default=1,
metavar='[ith joint]', type=int,
help='which joint to set the desired angle (starting from 1st)')
parser.add_argument('-a','--alpha', default=-0.5,
metavar='[value]', type=float,help='stepsize scaling parameter alpha')
args = parser.parse_args()
q_angle_change_rad = args.q_angle_change/180.0*np.pi
q_index = args.index
alpha = -abs(args.alpha)
moveit_commander.roscpp_initialize('')
rospy.init_node('robot_jogging')
client = actionlib.SimpleActionClient('/xarm/xarm7_traj_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
client.wait_for_server()
arm_group_name = "xarm7"
robot = moveit_commander.RobotCommander("robot_description")
scene = moveit_commander.PlanningSceneInterface(ns=rospy.get_namespace())
arm_group = moveit_commander.MoveGroupCommander(arm_group_name, ns=rospy.get_namespace())
display_trajectory_publisher = rospy.Publisher(rospy.get_namespace() + 'move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
redundancy_resolution(q_angle_change_rad, q_index, alpha, client)
def ee_move():
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('simple3', anonymous=True)
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("manipulator")
group.set_planner_id('RRTkConfigDefault')
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory)
print "============ Reference frame: %s" % group.get_planning_frame()
print "============ Waiting for RVIZ..."
rospy.sleep(5)
print "============ Starting script"
## Get current EE Pose - RPY
print "Current ee_link Pose is:"
print group.get_current_pose(end_effector_link="ee_link")
## MOVEMENT IN X
group.shift_pose_target(0, -0.25, end_effector_link="ee_link")
print "============ Waiting while RVIZ displays x movement..."
rospy.sleep(5)
#group.plan()
## Get current EE Pose - RPY
#print "New ee_link Pose is (changed x):"
#print group.get_current_pose(end_effector_link = "ee_link")
## MOVEMENT IN Y
#group.shift_pose_target(1,0.05,end_effector_link = "ee_link")
#print "============ Waiting while RVIZ displays y movement..."
#rospy.sleep(5)
group.go(wait=True)
## Get current EE Pose - RPY
#print "New ee_link Pose is (changed y):"
#print group.get_current_pose(end_effector_link = "ee_link")
## MOVEMENT IN Z
#print "Z target is:"
#print z_arm_target
#group.shift_pose_target(2,0.05,end_effector_link = "ee_link")
#print "============ Waiting while RVIZ displays z movement..."
#rospy.sleep(5)
#group.go(wait=True)
## Get current EE Pose - RPY
#print "New ee_link Pose is (changed z):"
#print group.get_current_pose(end_effector_link = "ee_link")
#collision_object = moveit_msgs.msg.CollisionObject()
moveit_commander.roscpp_shutdown()
#rospy.spin()
print "============ STOPPING"
#! /usr/bin/env python
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("manipulator")
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory)
while (True):
print("Reference frame", group.get_planning_frame())
print("End Effector", group.get_end_effector_link())
print("Current Pose:", group.get_current_pose())
print("Robot State", robot.get_current_state())
print("--------------------------------------------------")
print("--------------------------------------------------")
rospy.sleep(3)
def __init__(self, limb='right', tip_name="right_gripper_tip"):
# print("Class init happening bro!")
super(PickAndPlace, self).__init__()
joint_state_topic = ['joint_states:=/robot/joint_states']\
# at HOME position, orientation of gripper frame w.r.t world x=0.7, y=0.7, z=0.0, w=0.0 or [ rollx: -3.1415927, pitchy: 0, yawz: -1.5707963 ]
# (roll about an X-axis w.r.t home) / (subsequent pitch about the Y-axis) / (subsequent yaw about the Z-axis)
rollx = -3.1415926535
pitchy = 0.0
yawz = 0.0 # -1.5707963
# use q with moveit because giving quaternion.x doesn't work.
q = quaternion_from_euler(rollx, pitchy, yawz)
overhead_orientation_moveit = Quaternion(x=q[0],
y=q[1],
z=q[2],
w=q[3])
moveit_commander.roscpp_initialize(joint_state_topic)
rospy.init_node('pnp_node', anonymous=True, disable_signals=False)
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = "right_arm"
group = moveit_commander.MoveGroupCommander(group_name)
# See ompl_planning.yaml for a complete list
group.set_planner_id("RRTConnectkConfigDefault")
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
planning_frame = group.get_planning_frame()
eef_link = group.get_end_effector_link()
group_names = robot.get_group_names()
req = AttachRequest()
# print(robot.get_current_state())
# Misc variables
self.robot = robot
self.scene = scene
self.group = group
self.req = req
self.display_trajectory_publisher = display_trajectory_publisher
self.planning_frame = planning_frame
self.eef_link = eef_link
self.group_names = group_names
self._limb_name = limb # string
self._limb = intera_interface.Limb(limb)
self._tip_name = tip_name
self.q = q
self.overhead_orientation_moveit = overhead_orientation_moveit
self.target_location_x = -100
self.target_location_y = -100
self.target_location_z = -100
self.onion_index = 0
self.num_onions = 0
self.bad_onions = []
self.onionLoc = None
self.eefLoc = None
self.prediction = None
self.listIDstatus = None
