def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Initialize the move group for the right arm
right_arm = MoveGroupCommander('right_arm')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.02)
right_arm.set_goal_orientation_tolerance(0.1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
# Move to the named "straight_forward" pose stored in the SRDF file
right_arm.set_named_target('straight_forward')
right_arm.go()
rospy.sleep(2)
# Move back to the resting position at roughly 1/4 speed
right_arm.set_named_target('resting')
# Get back the planned trajectory
traj = right_arm.plan()
# Scale the trajectory speed by a factor of 0.25
new_traj = scale_trajectory_speed(traj, 0.25)
# Execute the new trajectory
right_arm.execute(new_traj)
rospy.sleep(1)
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
right_arm = MoveGroupCommander("right_arm")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
print right_arm.get_current_pose().pose
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 0.46822292532
wpose.orientation.x = -0.0855260625632
wpose.orientation.y = -0.781945671575
wpose.orientation.z = -0.402509070125
wpose.position.y = 0.0924247084452
wpose.position.z = 1.64720495919
wpose.position.x = -0.0834555596048
right_arm.set_pose_target(wpose)
plan1 = right_arm.plan()
right_arm.execute(plan1)
print "============ Waiting while RVIZ executes plan1..."
rospy.sleep(5)
waypoints = []
waypoints.append(right_arm.get_current_pose().pose)
print right_arm.get_current_pose().pose
points = 10
for i in xrange(points):
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = waypoints[i-1].orientation.w
wpose.orientation.x = waypoints[i-1].orientation.x
wpose.orientation.y = waypoints[i-1].orientation.y
wpose.orientation.z = waypoints[i-1].orientation.z
wpose.position.y = waypoints[i-1].position.y
wpose.position.z = waypoints[i-1].position.z - 0.05
right_arm = MoveGroupCommander("right_arm")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
print right_arm.get_current_pose().pose
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 0.570398010274
wpose.orientation.x = -0.279748061111
wpose.orientation.y = -0.719670670452
wpose.orientation.z = -0.280109368412
wpose.position.y = -0.0349149588619
wpose.position.z = 1.69789257029
wpose.position.x = 0.0117939632591
right_arm.set_pose_target(wpose)
plan1 = right_arm.plan()
right_arm.execute(plan1)
print "============ Waiting while RVIZ executes plan1..."
rospy.sleep(5)
waypoints = []
waypoints.append(right_arm.get_current_pose().pose)
print right_arm.get_current_pose().pose
gain = 0.05;
r = 0.05
a = 0.0117939632591 # waypoints[0].position.x
b = 1.64789257 # waypoints[0].position.z
#waypoints[0].position.x = a + r
points = 10
for i in xrange(points):
wpose = geometry_msgs.msg.Pose()
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('dianaS1_arm')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.001)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_max_velocity_scaling_factor(0.5)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 获取当前位姿数据最为机械臂运动的起始位姿
start_pose = arm.get_current_pose(end_effector_link).pose
# 初始化路点列表
waypoints = []
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
wpose.position.z -= 0.3
waypoints.append(deepcopy(wpose))
wpose.position.y += 0.4
waypoints.append(deepcopy(wpose))
wpose.position.y -= 0.4
wpose.position.z += 0.3
waypoints.append(deepcopy(wpose))
# 规划过程
fraction = 0.0 # 路径规划覆盖率
maxtries = 100 # 最大尝试规划次数
attempts = 0 # 已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
# 规划路径 ，fraction返回1代表规划成功
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses，路点列表，这里是5个点
0.01, # eef_step，终端步进值，每隔0.01m计算一次逆解判断能否可达
0.0, # jump_threshold，跳跃阈值，设置为0代表不允许跳跃
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(maxtries) + " attempts.")
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('move_continue_demo', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
robot = moveit_commander.RobotCommander()
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
# arm.set_goal_position_tolerance(0.01)
# arm.set_goal_orientation_tolerance(0.1)
arm.set_max_velocity_scaling_factor(0.5)
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_start_state_to_current_state()
arm.set_named_target("fusion_left")
arm.go()
# 轨迹速度加速度缩放
def scale_trajectory_vel_acc(traj, vel_scale, acc_scale):
new_traj = RobotTrajectory()
new_traj.joint_trajectory = traj.joint_trajectory
n_joints = len(traj.joint_trajectory.joint_names)
n_points = len(traj.joint_trajectory.points)
points = list(traj.joint_trajectory.points)
for i in range(n_points):
point = JointTrajectoryPoint()
point.positions = traj.joint_trajectory.points[i].positions
point.time_from_start = traj.joint_trajectory.points[
i].time_from_start / vel_scale
point.velocities = list(
traj.joint_trajectory.points[i].velocities)
point.accelerations = list(
traj.joint_trajectory.points[i].accelerations)
for j in range(n_joints):
point.velocities[j] = point.velocities[j] * vel_scale
point.accelerations[
j] = point.accelerations[j] * acc_scale * acc_scale
points[i] = point
new_traj.joint_trajectory.points = points
return new_traj
# 重新计算轨迹时间
def retime_trajectory(plan, scale):
ref_state = robot.get_current_state()
retimed_plan = arm.retime_trajectory(ref_state,
plan,
velocity_scaling_factor=scale)
return retimed_plan
# 拼接轨迹点
def stitch_positions(positions):
plan_list = []
state = robot.get_current_state()
# 路径规划, 连接各路径点
for i in range(len(positions)):
# 设置前一路径点为待规划的路径起点, 起始点除外
if i > 0:
state.joint_state.position = positions[i - 1]
arm.set_start_state(state)
# 设置目标状态
arm.set_joint_value_target(positions[i])
plan = arm.plan()
plan_list.append(plan)
# 创建新轨迹, 重新计算时间
new_traj = RobotTrajectory()
new_traj.joint_trajectory.joint_names = plan_list[
0].joint_trajectory.joint_names
# 轨迹点拼接
new_points = []
for plan in plan_list:
new_points += list(plan.joint_trajectory.points)
new_traj.joint_trajectory.points = new_points
# 重新计算轨迹时间
new_traj = retime_trajectory(new_traj, scale=0.5)
return new_traj
# positions = [[0.055, 0.317, -0.033, -1.292, -0.099, -0.087, -1.575],
# [1.055, 1.317, -1.033, -1.292, -0.099, -0.087, -1.575],
# [1.055, -1.317, 1.033, 1.292, -0.099, 1.087, -1.575]]
#
# plan = stitch_positions(positions)
# arm.execute(plan)
positions_name = [
'cali_3', 'fusion_left2', 'fusion_left1', 'fusion_1',
'fusion_right1', 'fusion_right2'
]
positions = []
for name in positions_name:
position_dict = arm.get_named_target_values(name)
joint_names = sorted(position_dict.keys())
position = []
for joint_name in joint_names:
position.append(position_dict[joint_name])
positions.append(position)
plan = stitch_positions(positions)
# 速度加速度缩放
# plan = scale_trajectory_vel_acc(plan, 0.25, 0.25)
# new_traj = retime_trajectory(plan, scale=0.25)
arm.execute(plan)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
exit()
# arm.get_named_target_values()
# ########################################### test #############################################
# 轨迹列表
plan_list = []
# 设置初始状态
state = robot.get_current_state()
arm.set_start_state(state)
# 设置目标状态
aim_position1 = [0.055, 0.317, -0.033, -1.292, -0.099, -0.087, -1.575]
arm.set_joint_value_target(aim_position1)
plan1 = arm.plan()
plan_list.append(plan1)
# 设置初始状态
state.joint_state.position = aim_position1
arm.set_start_state(state)
# 设置目标状态
aim_position2 = [1.055, 1.317, -1.033, -1.292, -0.099, -0.087, -1.575]
arm.set_joint_value_target(aim_position2)
plan2 = arm.plan()
plan_list.append(plan2)
# 设置初始状态
state.joint_state.position = aim_position2
arm.set_start_state(state)
# 设置目标状态
aim_position3 = [1.055, -1.317, 1.033, 1.292, -0.099, 1.087, -1.575]
arm.set_joint_value_target(aim_position3)
plan3 = arm.plan()
plan_list.append(plan3)
new_traj = RobotTrajectory()
new_traj.joint_trajectory.joint_names = plan1.joint_trajectory.joint_names
# 轨迹点拼接
new_points = []
for plan in plan_list:
new_points += list(plan.joint_trajectory.points)
new_traj.joint_trajectory.points = new_points
# 重新计算轨迹时间
new_traj = retime_trajectory(new_traj, scale=0.5)
# 执行轨迹
arm.execute(new_traj)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('ur_arm')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂运动到之前设置的“forward”姿态
arm.set_named_target('forward')
arm.go()
# 获取当前位姿数据最为机械臂运动的起始位姿
start_pose = arm.get_current_pose(end_effector_link).pose
# 初始化路点列表
waypoints = []
# 将初始位姿加入路点列表
if cartesian:
waypoints.append(start_pose)
# 设置第二个路点数据，并加入路点列表
# 第二个路点需要向后运动0.2米，向右运动0.2米
wpose = deepcopy(start_pose)
wpose.position.x -= 0.2
wpose.position.y -= 0.2
if cartesian:
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# 设置第三个路点数据，并加入路点列表
wpose.position.y += 0.1
if cartesian:
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# 设置第四个路点数据，回到初始位置，并加入路点列表
if cartesian:
waypoints.append(deepcopy(start_pose))
else:
arm.set_pose_target(start_pose)
arm.go()
rospy.sleep(1)
if cartesian:
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses，路点列表
0.01, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " +
str(fraction) + " success after " +
str(maxtries) + " attempts.")
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class SmartGrasper(object):
"""
This is the helper library to easily access the different functionalities of the simulated robot
from python.
"""
__last_joint_state = None
def __init__(self):
"""
This constructor initialises the different necessary connections to the topics and services
and resets the world to start in a good position.
"""
rospy.init_node("smart_grasper")
self.__joint_state_sub = rospy.Subscriber("/joint_states",
JointState,
self.__joint_state_cb,
queue_size=1)
rospy.wait_for_service("/gazebo/get_model_state", 10.0)
rospy.wait_for_service("/gazebo/reset_world", 10.0)
self.__reset_world = rospy.ServiceProxy("/gazebo/reset_world", Empty)
self.__get_pose_srv = rospy.ServiceProxy("/gazebo/get_model_state",
GetModelState)
rospy.wait_for_service("/gazebo/pause_physics")
self.__pause_physics = rospy.ServiceProxy("/gazebo/pause_physics",
Empty)
rospy.wait_for_service("/gazebo/unpause_physics")
self.__unpause_physics = rospy.ServiceProxy("/gazebo/unpause_physics",
Empty)
rospy.wait_for_service("/controller_manager/switch_controller")
self.__switch_ctrl = rospy.ServiceProxy(
"/controller_manager/switch_controller", SwitchController)
rospy.wait_for_service("/gazebo/set_model_configuration")
self.__set_model = rospy.ServiceProxy(
"/gazebo/set_model_configuration", SetModelConfiguration)
rospy.wait_for_service('/get_planning_scene', 10.0)
self.__get_planning_scene = rospy.ServiceProxy('/get_planning_scene',
GetPlanningScene)
self.__pub_planning_scene = rospy.Publisher('/planning_scene',
PlanningScene,
queue_size=10,
latch=True)
self.arm_commander = MoveGroupCommander("arm")
self.hand_commander = MoveGroupCommander("hand")
self.__hand_traj_client = SimpleActionClient(
"/hand_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
self.__arm_traj_client = SimpleActionClient(
"/arm_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
if self.__hand_traj_client.wait_for_server(
timeout=rospy.Duration(4.0)) is False:
rospy.logfatal(
"Failed to connect to /hand_controller/follow_joint_trajectory in 4sec."
)
raise Exception(
"Failed to connect to /hand_controller/follow_joint_trajectory in 4sec."
)
if self.__arm_traj_client.wait_for_server(
timeout=rospy.Duration(4.0)) is False:
rospy.logfatal(
"Failed to connect to /arm_controller/follow_joint_trajectory in 4sec."
)
raise Exception(
"Failed to connect to /arm_controller/follow_joint_trajectory in 4sec."
)
self.reset_world()
def reset_world(self):
"""
Resets the object poses in the world and the robot joint angles.
"""
self.__switch_ctrl.call(start_controllers=[],
stop_controllers=[
"hand_controller", "arm_controller",
"joint_state_controller"
],
strictness=SwitchControllerRequest.BEST_EFFORT)
self.__pause_physics.call()
joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint', 'H1_WRJ1'
]
joint_positions = [1.2, 0.5, -1.5, -0.5, -1.6, -0.3, 0.]
self.__set_model.call(model_name="smart_grasping_sandbox",
urdf_param_name="robot_description",
joint_names=joint_names,
joint_positions=joint_positions)
timer = Timer(0.0, self.__start_ctrl)
timer.start()
time.sleep(0.1)
self.__unpause_physics.call()
self.__reset_world.call()
def get_ball_pose(self):
"""
Gets the pose of the ball in the world frame.
@return The pose of the ball.
"""
return self.__get_pose_srv.call("cricket_ball", "world").pose
def get_tip_pose(self):
"""
Gets the current pose of the robot's tooltip in the world frame.
@return the tip pose
"""
return self.arm_commander.get_current_pose(
self.arm_commander.get_end_effector_link()).pose
def move_tip_absolute(self, target):
"""
Moves the tooltip to the absolute target in the world frame
@param target is a geometry_msgs.msg.Pose
@return True on success
"""
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([target])
plan = self.arm_commander.plan()
if not self.arm_commander.execute(plan):
return False
return True
def move_tip(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0.):
"""
Moves the tooltip in the world frame by the given x,y,z / roll,pitch,yaw.
@return True on success
"""
transform = PyKDL.Frame(PyKDL.Rotation.RPY(pitch, roll, yaw),
PyKDL.Vector(-x, -y, -z))
tip_pose = self.get_tip_pose()
tip_pose_kdl = posemath.fromMsg(tip_pose)
final_pose = toMsg(tip_pose_kdl * transform)
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([final_pose])
plan = self.arm_commander.plan()
if not self.arm_commander.execute(plan):
return False
return True
def send_command(self, command, duration=0.2):
"""
Send a dictionnary of joint targets to the arm and hand directly.
@param command: a dictionnary of joint names associated with a target:
{"H1_F1J1": -1.0, "shoulder_pan_joint": 1.0}
@param duration: the amount of time it will take to get there in seconds. Needs to be bigger than 0.0
"""
hand_goal = None
arm_goal = None
for joint, target in command.items():
if "H1" in joint:
if not hand_goal:
hand_goal = FollowJointTrajectoryGoal()
point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration.from_sec(duration)
hand_goal.trajectory.points.append(point)
hand_goal.trajectory.joint_names.append(joint)
hand_goal.trajectory.points[0].positions.append(target)
else:
if not arm_goal:
arm_goal = FollowJointTrajectoryGoal()
point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration.from_sec(duration)
arm_goal.trajectory.points.append(point)
arm_goal.trajectory.joint_names.append(joint)
arm_goal.trajectory.points[0].positions.append(target)
if arm_goal:
self.__arm_traj_client.send_goal_and_wait(arm_goal)
if hand_goal:
self.__hand_traj_client.send_goal_and_wait(hand_goal)
def get_current_joint_state(self):
"""
Gets the current state of the robot.
@return joint positions, velocity and efforts as three dictionnaries
"""
joints_position = {
n: p
for n, p in zip(self.__last_joint_state.name,
self.__last_joint_state.position)
}
joints_velocity = {
n: v
for n, v in zip(self.__last_joint_state.name,
self.__last_joint_state.velocity)
}
joints_effort = {
n: v
for n, v in zip(self.__last_joint_state.name,
self.__last_joint_state.effort)
}
return joints_position, joints_velocity, joints_effort
def open_hand(self):
"""
Opens the hand.
@return True on success
"""
self.hand_commander.set_named_target("open")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
return True
def close_hand(self):
"""
Closes the hand.
@return True on success
"""
self.hand_commander.set_named_target("close")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
return True
def check_fingers_collisions(self, enable=True):
"""
Disables or enables the collisions check between the fingers and the objects / table
@param enable: set to True to enable / False to disable
@return True on success
"""
objects = ["cricket_ball__link", "drill__link", "cafe_table__link"]
while self.__pub_planning_scene.get_num_connections() < 1:
rospy.loginfo(
"waiting for someone to subscribe to the /planning_scene")
rospy.sleep(0.1)
request = PlanningSceneComponents(
components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX)
response = self.__get_planning_scene(request)
acm = response.scene.allowed_collision_matrix
for object_name in objects:
if object_name not in acm.entry_names:
# add object to allowed collision matrix
acm.entry_names += [object_name]
for row in range(len(acm.entry_values)):
acm.entry_values[row].enabled += [False]
new_row = deepcopy(acm.entry_values[0])
acm.entry_values += {new_row}
for index_entry_values, entry_values in enumerate(acm.entry_values):
if "H1_F" in acm.entry_names[index_entry_values]:
for index_value, _ in enumerate(entry_values.enabled):
if acm.entry_names[index_value] in objects:
if enable:
acm.entry_values[index_entry_values].enabled[
index_value] = False
else:
acm.entry_values[index_entry_values].enabled[
index_value] = True
elif acm.entry_names[index_entry_values] in objects:
for index_value, _ in enumerate(entry_values.enabled):
if "H1_F" in acm.entry_names[index_value]:
if enable:
acm.entry_values[index_entry_values].enabled[
index_value] = False
else:
acm.entry_values[index_entry_values].enabled[
index_value] = True
planning_scene_diff = PlanningScene(is_diff=True,
allowed_collision_matrix=acm)
self.__pub_planning_scene.publish(planning_scene_diff)
rospy.sleep(1.0)
return True
def pick(self):
"""
Does its best to pick the ball.
"""
rospy.loginfo("Moving to Pregrasp")
self.open_hand()
time.sleep(0.1)
ball_pose = self.get_ball_pose()
ball_pose.position.z += 0.5
#setting an absolute orientation (from the top)
quaternion = quaternion_from_euler(-pi / 2., 0.0, 0.0)
ball_pose.orientation.x = quaternion[0]
ball_pose.orientation.y = quaternion[1]
ball_pose.orientation.z = quaternion[2]
ball_pose.orientation.w = quaternion[3]
self.move_tip_absolute(ball_pose)
rospy.loginfo("Grasping")
self.move_tip(y=-0.093)
self.check_fingers_collisions(False)
self.close_hand()
rospy.loginfo("Lifting")
for _ in range(50):
self.move_tip(y=0.001)
time.sleep(0.1)
self.check_fingers_collisions(True)
def __compute_arm_target_for_ball(self):
ball_pose = self.get_ball_pose()
# come at it from the top
arm_target = ball_pose
arm_target.position.z += 0.5
quaternion = quaternion_from_euler(-pi / 2., 0.0, 0.0)
arm_target.orientation.x = quaternion[0]
arm_target.orientation.y = quaternion[1]
arm_target.orientation.z = quaternion[2]
arm_target.orientation.w = quaternion[3]
return arm_target
def __pre_grasp(self, arm_target):
self.hand_commander.set_named_target("open")
plan = self.hand_commander.plan()
self.hand_commander.execute(plan, wait=True)
for _ in range(10):
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([arm_target])
plan = self.arm_commander.plan()
if self.arm_commander.execute(plan):
return True
def __grasp(self, arm_target):
waypoints = []
waypoints.append(
self.arm_commander.get_current_pose(
self.arm_commander.get_end_effector_link()).pose)
arm_above_ball = deepcopy(arm_target)
arm_above_ball.position.z -= 0.12
waypoints.append(arm_above_ball)
self.arm_commander.set_start_state_to_current_state()
(plan, fraction) = self.arm_commander.compute_cartesian_path(
waypoints, 0.01, 0.0)
print fraction
if not self.arm_commander.execute(plan):
return False
self.hand_commander.set_named_target("close")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
self.hand_commander.attach_object("cricket_ball__link")
def __lift(self, arm_target):
waypoints = []
waypoints.append(
self.arm_commander.get_current_pose(
self.arm_commander.get_end_effector_link()).pose)
arm_above_ball = deepcopy(arm_target)
arm_above_ball.position.z += 0.1
waypoints.append(arm_above_ball)
self.arm_commander.set_start_state_to_current_state()
(plan, fraction) = self.arm_commander.compute_cartesian_path(
waypoints, 0.01, 0.0)
print fraction
if not self.arm_commander.execute(plan):
return False
def __start_ctrl(self):
rospy.loginfo("STARTING CONTROLLERS")
self.__switch_ctrl.call(start_controllers=[
"hand_controller", "arm_controller", "joint_state_controller"
],
stop_controllers=[],
strictness=1)
def __joint_state_cb(self, msg):
self.__last_joint_state = msg
right_arm = MoveGroupCommander("right_arm")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
print right_arm.get_current_pose().pose
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 0.445677565944
wpose.orientation.x = -0.326257686982
wpose.orientation.y = -0.770762217792
wpose.orientation.z = -0.31757366472
wpose.position.y = 0.172181465244
wpose.position.z = 1.57438900065
wpose.position.x = -0.136240808288
right_arm.set_pose_target(wpose)
plan1 = right_arm.plan()
right_arm.execute(plan1)
print "============ Waiting while RVIZ executes plan1..."
rospy.sleep(5)
waypoints = []
waypoints.append(right_arm.get_current_pose().pose)
print right_arm.get_current_pose().pose
points = 20
for i in xrange(points):
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = waypoints[i-1].orientation.w
wpose.orientation.x = waypoints[i-1].orientation.x
wpose.orientation.y = waypoints[i-1].orientation.y
wpose.orientation.z = waypoints[i-1].orientation.z
wpose.position.y = waypoints[i-1].position.y - 0.15
class SrRobotCommander(object):
"""
Base class for hand and arm commanders.
"""
def __init__(self, name):
"""
Initialize MoveGroupCommander object.
@param name - name of the MoveIt group.
"""
self._name = name
self._move_group_commander = MoveGroupCommander(name)
self._robot_commander = RobotCommander()
self._robot_name = self._robot_commander._r.get_robot_name()
self.refresh_named_targets()
self._warehouse_name_get_srv = rospy.ServiceProxy("get_robot_state",
GetState)
self._planning_scene = PlanningSceneInterface()
self._joint_states_lock = threading.Lock()
self._joint_states_listener = \
rospy.Subscriber("joint_states", JointState,
self._joint_states_callback, queue_size=1)
self._joints_position = {}
self._joints_velocity = {}
self._joints_effort = {}
self._joints_state = None
self._clients = {}
self.__plan = None
self._controllers = {}
rospy.wait_for_service('compute_ik')
self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK)
controller_list_param = rospy.get_param("/move_group/controller_list")
# create dictionary with name of controllers and corresponding joints
self._controllers = {item["name"]: item["joints"] for item in controller_list_param}
self._set_up_action_client(self._controllers)
self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer)
threading.Thread(None, rospy.spin)
def _is_trajectory_valid(self, trajectory, required_keys):
if type(trajectory) != list:
rospy.logerr("Trajectory is not a list of waypoints")
return False
no_error = True
for k in required_keys:
if "|" in k:
optional = k.split("|")
if len(set(optional).intersection(set(trajectory[0].keys()))) == 0:
rospy.logerr("Trajectory is missing both of {} keys".format(optional))
no_error = False
else:
if k not in list(trajectory[0].keys()):
rospy.logerr("Trajectory waypoint missing {}".format(k))
no_error = False
return no_error
def set_planner_id(self, planner_id):
"""
Sets the planner_id used for all future planning requests.
@param planner_id - The string for the planner id, set to None to clear.
"""
self._move_group_commander.set_planner_id(planner_id)
def set_num_planning_attempts(self, num_planning_attempts):
self._move_group_commander.set_num_planning_attempts(num_planning_attempts)
def set_planning_time(self, seconds):
"""
Specifies the amount of time to be used for motion planning.
Some planning algorithms might require more time than others to compute
a valid solution.
"""
self._move_group_commander.set_planning_time(seconds)
def get_end_effector_pose_from_named_state(self, name):
state = self._warehouse_name_get_srv(name, self._robot_name).state
return self.get_end_effector_pose_from_state(state)
def get_end_effector_pose_from_state(self, state):
header = Header()
fk_link_names = [self._move_group_commander.get_end_effector_link()]
header.frame_id = self._move_group_commander.get_pose_reference_frame()
response = self._forward_k(header, fk_link_names, state)
return response.pose_stamped[0]
def get_planning_frame(self):
"""
@return - returns the name of the frame wrt which the motion planning
is computed.
"""
return self._move_group_commander.get_planning_frame()
def set_pose_reference_frame(self, reference_frame):
"""
Set the reference frame to assume for poses of end-effectors.
@param reference_frame - name of the frame.
"""
self._move_group_commander.set_pose_reference_frame(reference_frame)
def get_group_name(self):
return self._name
def refresh_named_targets(self):
self._srdf_names = self.__get_srdf_names()
self._warehouse_names = self.__get_warehouse_names()
def set_max_velocity_scaling_factor(self, value):
"""
Set a scaling factor for optionally reducing the maximum joint velocity.
@param value - Allowed values are in (0,1].
"""
self._move_group_commander.set_max_velocity_scaling_factor(value)
def set_max_acceleration_scaling_factor(self, value):
"""
Set a scaling factor for optionally reducing the maximum joint accelaration.
@param value - Allowed values are in (0,1].
"""
self._move_group_commander.set_max_acceleration_scaling_factor(value)
def allow_looking(self, value):
"""
Enable/disable looking around for motion planning.
@param value - boolean.
"""
self._move_group_commander.allow_looking(value)
def allow_replanning(self, value):
"""
Enable/disable replanning in case new obstacles are detected
while executing a plan.
@param value - boolean.
"""
self._move_group_commander.allow_replanning(value)
def execute(self):
"""
Executes the last plan made.
@return - Success of execution.
"""
is_executed = False
if self.check_plan_is_valid():
is_executed = self._move_group_commander.execute(self.__plan)
self.__plan = None
else:
rospy.logwarn("No plans were made, not executing anything.")
if not is_executed:
rospy.logerr("Execution failed.")
else:
rospy.loginfo("Execution succeeded.")
return is_executed
def execute_plan(self, plan):
"""
Executes a given plan.
@param plan - RobotTrajectory msg that contains the trajectory
to the set goal state.
@return - Success of execution.
"""
is_executed = False
if self.check_given_plan_is_valid(plan):
is_executed = self._move_group_commander.execute(plan)
self.__plan = None
else:
rospy.logwarn("Plan is not valid, not executing anything.")
if not is_executed:
rospy.logerr("Execution failed.")
else:
rospy.loginfo("Execution succeeded.")
return is_executed
def move_to_joint_value_target(self, joint_states, wait=True,
angle_degrees=False):
"""
Set target of the robot's links and moves to it.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param wait - should method wait for movement end or not.
@param angle_degrees - are joint_states in degrees or not.
"""
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update((joint, radians(i))
for joint, i in joint_states_cpy.items())
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_joint_value_target(joint_states_cpy)
self._move_group_commander.go(wait=wait)
def set_start_state_to_current_state(self):
return self._move_group_commander.set_start_state_to_current_state()
def plan_to_joint_value_target(self, joint_states, angle_degrees=False, custom_start_state=None):
"""
Set target of the robot's links and plans.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param angle_degrees - are joint_states in degrees or not.
@param custom_start_state - specify a start state different than the current state
This is a blocking method.
@return - motion plan (RobotTrajectory msg) that contains the trajectory to the set goal state.
"""
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update((joint, radians(i))
for joint, i in joint_states_cpy.items())
if custom_start_state is None:
self._move_group_commander.set_start_state_to_current_state()
else:
self._move_group_commander.set_start_state(custom_start_state)
self._move_group_commander.set_joint_value_target(joint_states_cpy)
self.__plan = self._move_group_commander.plan()[CONST_TUPLE_TRAJECTORY_INDEX]
return self.__plan
def check_plan_is_valid(self):
"""
Checks if current plan contains a valid trajectory
"""
return (self.__plan is not None and len(self.__plan.joint_trajectory.points) > 0)
def check_given_plan_is_valid(self, plan):
"""
Checks if given plan contains a valid trajectory
"""
return (plan is not None and len(plan.joint_trajectory.points) > 0)
def evaluate_given_plan(self, plan):
"""
Returns given plan quality calculated by a weighted sum of angles traveled by each
of the joints, giving higher weights to the joints closer to the base of the robot,
thus penalizing them as smallmovements of these joints will result in bigger movements
of the end effector. Formula:
PQ = sum_(i=0)^(n-1){w_i * abs(x_i - x_(i0)}, where:
n - number of robot's joints,
w - weight specified for each joint,
x - joint's goal position,
x_0 - joint's initial position.
The lower the value, the better the plan.
"""
if plan is None:
return None
num_of_joints = len(plan.joint_trajectory.points[0].positions)
weights = numpy.array(sorted(range(1, num_of_joints + 1), reverse=True))
plan_array = numpy.empty(shape=(len(plan.joint_trajectory.points),
num_of_joints))
for i, point in enumerate(plan.joint_trajectory.points):
plan_array[i] = point.positions
deltas = abs(numpy.diff(plan_array, axis=0))
sum_deltas = numpy.sum(deltas, axis=0)
sum_deltas_weighted = sum_deltas * weights
plan_quality = float(numpy.sum(sum_deltas_weighted))
return plan_quality
def evaluate_plan(self):
return self.evaluate_given_plan(self.__plan)
def evaluate_plan_quality(self, plan_quality, good_threshold=20, medium_threshold=50):
if plan_quality > medium_threshold:
rospy.logwarn("Low plan quality! Value: {}".format(plan_quality))
return 'poor'
elif (plan_quality > good_threshold and plan_quality < medium_threshold):
rospy.loginfo("Medium plan quality. Value: {}".format(plan_quality))
return 'medium'
elif plan_quality < good_threshold:
rospy.loginfo("Good plan quality. Value: {}".format(plan_quality))
return 'good'
def get_robot_name(self):
return self._robot_name
def named_target_in_srdf(self, name):
return name in self._srdf_names
def set_named_target(self, name):
"""
Set a joint configuration by name.
@param name - name of the target which must correspond to a name defined,
either in the srdf or in the mongo warehouse database.
@return - bool to confirm that the target has been correctly set.
"""
if name in self._srdf_names:
self._move_group_commander.set_named_target(name)
elif (name in self._warehouse_names):
response = self._warehouse_name_get_srv(name, self._robot_name)
active_names = self._move_group_commander._g.get_active_joints()
joints = response.state.joint_state.name
positions = response.state.joint_state.position
js = {}
for n, this_name in enumerate(joints):
if this_name in active_names:
js[this_name] = positions[n]
try:
self._move_group_commander.set_joint_value_target(js)
except Exception as e:
rospy.loginfo(e)
else:
rospy.logerr("Unknown named state '%s'..." % name)
return False
return True
def get_named_target_joint_values(self, name):
"""
Get the joint angles for targets specified by name.
@param name - @param name - name of the target which must correspond to a name defined,
either in the srdf or in the mongo warehouse database.
@return - joint values of the named target.
"""
output = dict()
if (name in self._srdf_names):
output = self._move_group_commander._g.get_named_target_values(str(name))
elif (name in self._warehouse_names):
js = self._warehouse_name_get_srv(
name, self._robot_name).state.joint_state
for x, n in enumerate(js.name):
if n in self._move_group_commander._g.get_joints():
output[n] = js.position[x]
else:
rospy.logerr("No target named %s" % name)
return None
return output
def get_end_effector_link(self):
return self._move_group_commander.get_end_effector_link()
def get_current_pose(self, reference_frame=None):
"""
Get the current pose of the end effector.
@param reference_frame - The desired reference frame in which end effector pose should be returned.
If none is passed, it will use the planning frame as reference.
@return - geometry_msgs.msg.Pose() - current pose of the end effector.
"""
if reference_frame is not None:
try:
trans = self.tf_buffer.lookup_transform(reference_frame,
self._move_group_commander.get_end_effector_link(),
rospy.Time(0),
rospy.Duration(5.0))
current_pose = geometry_msgs.msg.Pose()
current_pose.position.x = trans.transform.translation.x
current_pose.position.y = trans.transform.translation.y
current_pose.position.z = trans.transform.translation.z
current_pose.orientation.x = trans.transform.rotation.x
current_pose.orientation.y = trans.transform.rotation.y
current_pose.orientation.z = trans.transform.rotation.z
current_pose.orientation.w = trans.transform.rotation.w
return current_pose
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
rospy.logwarn("Couldn't get the pose from " + self._move_group_commander.get_end_effector_link() +
" in " + reference_frame + " reference frame")
return None
else:
return self._move_group_commander.get_current_pose().pose
def get_current_state(self):
"""
Get the current joint state of the group being used.
@return - a dictionary with the joint names as keys and current joint values.
"""
joint_names = self._move_group_commander._g.get_active_joints()
joint_values = self._move_group_commander._g.get_current_joint_values()
return dict(zip(joint_names, joint_values))
def get_current_state_bounded(self):
"""
Get the current joint state of the group being used, enforcing that they are within each joint limits.
@return - a dictionary with the joint names as keys and current joint values.
"""
current = self._move_group_commander._g.get_current_state_bounded()
names = self._move_group_commander._g.get_active_joints()
output = {n: current[n] for n in names if n in current}
return output
def get_robot_state_bounded(self):
return self._move_group_commander._g.get_current_state_bounded()
def move_to_named_target(self, name, wait=True):
"""
Set target of the robot's links and moves to it
@param name - name of the target pose defined in SRDF
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
if self.set_named_target(name):
self._move_group_commander.go(wait=wait)
def plan_to_named_target(self, name, custom_start_state=None):
"""
Set target of the robot's links and plans
This is a blocking method.
@param name - name of the target pose defined in SRDF.
@param custom_start_state - specify a start state different than the current state.
@return - a motion plan (RobotTrajectory msg) that contains the trajectory to the named target.
"""
if custom_start_state is None:
self._move_group_commander.set_start_state_to_current_state()
else:
self._move_group_commander.set_start_state(custom_start_state)
if self.set_named_target(name):
self.__plan = self._move_group_commander.plan()[CONST_TUPLE_TRAJECTORY_INDEX]
else:
rospy.logwarn("Could not find named target, plan not generated")
return False
return True
def __get_warehouse_names(self):
try:
list_srv = rospy.ServiceProxy("list_robot_states", ListStates)
return list_srv("", self._robot_name).states
except rospy.ServiceException as exc:
rospy.logwarn("Couldn't access warehouse: " + str(exc))
return list()
def _reset_plan(self):
self.__plan = None
def _set_plan(self, plan):
self.__plan = plan
def __get_srdf_names(self):
return self._move_group_commander._g.get_named_targets()
def get_named_targets(self):
"""
Get the complete list of named targets, from SRDF
as well as warehouse poses if available.
@return - list of strings containing names of targets.
"""
return self._srdf_names + self._warehouse_names
def get_joints_position(self):
"""
Returns joints position.
@return - dictionary with joints positions.
"""
with self._joint_states_lock:
return self._joints_position
def get_joints_velocity(self):
"""
Returns joints velocities
@return - dictionary with joints velocities.
"""
with self._joint_states_lock:
return self._joints_velocity
def _get_joints_effort(self):
"""
Returns joints effort.
@return - dictionary with joints efforts.
"""
with self._joint_states_lock:
return self._joints_effort
def get_joints_state(self):
"""
Returns joints state
@return - JointState message
"""
with self._joint_states_lock:
return self._joints_state
def run_joint_trajectory(self, joint_trajectory):
"""
Moves robot through all joint states with specified timeouts.
@param joint_trajectory - JointTrajectory class object. Represents
trajectory of the joints which would be executed.
@return - Success of execution.
"""
plan = RobotTrajectory()
plan.joint_trajectory = joint_trajectory
return self._move_group_commander.execute(plan)
def make_named_trajectory(self, trajectory):
"""
Makes joint value trajectory from specified by named poses (either from
SRDF or from warehouse).
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements (n.b either name or joint_angles is required)
- name -> the name of the way point
- joint_angles -> a dict of joint names and angles
- interpolate_time -> time to move from last wp
OPTIONAL:
- pause_time -> time to wait at this wp
- degrees -> set to true if joint_angles is specified in degrees. Assumed false if absent.
"""
if not self._is_trajectory_valid(trajectory, ["name|joint_angles", "interpolate_time"]):
return
current = self.get_current_state_bounded()
joint_trajectory = JointTrajectory()
joint_names = list(current.keys())
joint_trajectory.joint_names = joint_names
start = JointTrajectoryPoint()
start.positions = list(current.values())
start.time_from_start = rospy.Duration.from_sec(0.001)
joint_trajectory.points.append(start)
time_from_start = 0.0
for wp in trajectory:
joint_positions = None
if 'name' in wp.keys():
joint_positions = self.get_named_target_joint_values(wp['name'])
elif 'joint_angles' in wp.keys():
joint_positions = copy.deepcopy(wp['joint_angles'])
if 'degrees' in wp.keys() and wp['degrees']:
for joint, angle in joint_positions.items():
joint_positions[joint] = radians(angle)
if joint_positions is None:
rospy.logerr("Invalid waypoint. Must contain valid name for named target or dict of joint angles.")
return None
new_positions = {}
for n in joint_names:
new_positions[n] = joint_positions[n] if n in joint_positions else current[n]
trajectory_point = JointTrajectoryPoint()
trajectory_point.positions = [new_positions[n] for n in joint_names]
current = new_positions
time_from_start += wp['interpolate_time']
trajectory_point.time_from_start = rospy.Duration.from_sec(time_from_start)
joint_trajectory.points.append(trajectory_point)
if 'pause_time' in wp and wp['pause_time'] > 0:
extra = JointTrajectoryPoint()
extra.positions = trajectory_point.positions
time_from_start += wp['pause_time']
extra.time_from_start = rospy.Duration.from_sec(time_from_start)
joint_trajectory.points.append(extra)
return joint_trajectory
def send_stop_trajectory_unsafe(self):
"""
Sends a trajectory of all active joints at their current position.
This stops the robot.
"""
current = self.get_current_state_bounded()
trajectory_point = JointTrajectoryPoint()
trajectory_point.positions = list(current.values())
trajectory_point.time_from_start = rospy.Duration.from_sec(0.1)
trajectory = JointTrajectory()
trajectory.points.append(trajectory_point)
trajectory.joint_names = list(current.keys())
self.run_joint_trajectory_unsafe(trajectory)
def run_named_trajectory_unsafe(self, trajectory, wait=False):
"""
Moves robot through trajectory specified by named poses, either from
SRDF or from warehouse. Runs trajectory directly via contoller.
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements:
- name -> the name of the way point
- interpolate_time -> time to move from last wp
OPTIONAL:
- pause_time -> time to wait at this wp
"""
if self._is_trajectory_valid(trajectory, ['name|joint_angles', 'interpolate_time']):
joint_trajectory = self.make_named_trajectory(trajectory)
self.run_joint_trajectory_unsafe(joint_trajectory, wait)
def run_named_trajectory(self, trajectory):
"""
Moves robot through trajectory specified by named poses, either from
SRDF or from warehouse. Runs trajectory via moveit.
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements:
- name -> the name of the way point
- interpolate_time -> time to move from last wp
OPTIONAL:
- pause_time -> time to wait at this wp
"""
if self._is_trajectory_valid(trajectory, ['name|joint_angles', 'interpolate_time']):
joint_trajectory = self.make_named_trajectory(trajectory)
self.run_joint_trajectory(joint_trajectory)
def move_to_position_target(self, xyz, end_effector_link="", wait=True):
"""
Specify a target position for the end-effector and moves to it
preserving the current orientation of the end-effector.
@param xyz - new position of end-effector.
@param end_effector_link - name of the end effector link.
@param wait - should method wait for movement end or not.
"""
pose = self._move_group_commander.get_current_pose()
pose.pose.position.x = xyz[0]
pose.pose.position.y = xyz[1]
pose.pose.position.z = xyz[2]
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_pose_target(pose, end_effector_link)
self._move_group_commander.go(wait=wait)
def plan_to_position_target(self, xyz, end_effector_link="", custom_start_state=None):
"""
Specify a target position for the end-effector and plans preserving the current orientation of end-effector.
This is a blocking method.
@param xyz - new position of end-effector.
@param end_effector_link - name of the end effector link.
@param custom_start_state - specify a start state different than the current state.
"""
pose = self._move_group_commander.get_current_pose()
pose.pose.position.x = xyz[0]
pose.pose.position.y = xyz[1]
pose.pose.position.z = xyz[2]
if custom_start_state is None:
self._move_group_commander.set_start_state_to_current_state()
else:
self._move_group_commander.set_start_state(custom_start_state)
self._move_group_commander.set_pose_target(pose, end_effector_link)
self.__plan = self._move_group_commander.plan()[CONST_TUPLE_TRAJECTORY_INDEX]
return self.__plan
def move_to_pose_target(self, pose, end_effector_link="", wait=True):
"""
Specify a target pose for the end-effector and moves to it
@param pose - new pose of end-effector: a Pose message, a PoseStamped
message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
of 7 floats [x, y, z, qx, qy, qz, qw].
@param end_effector_link - name of the end effector link.
@param wait - should method wait for movement end or not.
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_pose_target(pose, end_effector_link)
self._move_group_commander.go(wait=wait)
def plan_to_pose_target(self, pose, end_effector_link="", alternative_method=False, custom_start_state=None):
"""
Specify a target pose for the end-effector and plans.
This is a blocking method.
@param pose - new pose of end-effector: a Pose message, a PoseStamped
message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
of 7 floats [x, y, z, qx, qy, qz, qw].
@param end_effector_link - name of the end effector link.
@param alternative_method - use set_joint_value_target instead of set_pose_target.
@param custom_start_state - specify a start state different than the current state.
"""
if custom_start_state is None:
self._move_group_commander.set_start_state_to_current_state()
else:
self._move_group_commander.set_start_state(custom_start_state)
if alternative_method:
self._move_group_commander.set_joint_value_target(pose, end_effector_link)
else:
self._move_group_commander.set_pose_target(pose, end_effector_link)
self.__plan = self._move_group_commander.plan()[CONST_TUPLE_TRAJECTORY_INDEX]
return self.__plan
def _joint_states_callback(self, joint_state):
"""
The callback function for the topic joint_states.
It will store the received joint position, velocity and efforts
information into dictionaries.
@param joint_state - the message containing the joints data.
"""
with self._joint_states_lock:
self._joints_state = joint_state
self._joints_position = {n: p for n, p in
zip(joint_state.name,
joint_state.position)}
self._joints_velocity = {n: v for n, v in
zip(joint_state.name,
joint_state.velocity)}
self._joints_effort = {n: v for n, v in
zip(joint_state.name, joint_state.effort)}
def _set_up_action_client(self, controller_list):
"""
Sets up an action client to communicate with the trajectory controller.
"""
self._action_running = {}
for controller_name in controller_list.keys():
self._action_running[controller_name] = False
service_name = controller_name + "/follow_joint_trajectory"
self._clients[controller_name] = SimpleActionClient(service_name,
FollowJointTrajectoryAction)
if self._clients[controller_name].wait_for_server(timeout=rospy.Duration(4)) is False:
err_msg = 'Failed to connect to action server ({}) in 4 sec'.format(service_name)
rospy.logwarn(err_msg)
def move_to_joint_value_target_unsafe(self, joint_states, time=0.002,
wait=True, angle_degrees=False):
"""
Set target of the robot's links and moves to it.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param time - time in s (counting from now) for the robot to reach the
target (it needs to be greater than 0.0 for it not to be rejected by
the trajectory controller).
@param wait - should method wait for movement end or not.
@param angle_degrees - are joint_states in degrees or not.
"""
# self._update_default_trajectory()
# self._set_targets_to_default_trajectory(joint_states)
goals = {}
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update((joint, radians(i))
for joint, i in joint_states_cpy.items())
for controller in self._controllers:
controller_joints = self._controllers[controller]
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = []
point = JointTrajectoryPoint()
point.positions = []
for x in joint_states_cpy.keys():
if x in controller_joints:
goal.trajectory.joint_names.append(x)
point.positions.append(joint_states_cpy[x])
point.time_from_start = rospy.Duration.from_sec(time)
goal.trajectory.points = [point]
goals[controller] = goal
self._call_action(goals)
if not wait:
return
for i in self._clients.keys():
if not self._clients[i].wait_for_result():
rospy.loginfo("Trajectory not completed")
def action_is_running(self, controller=None):
if controller is not None:
return self._action_running[controller]
for controller_running in self._action_running.values():
if controller_running:
return True
return False
def _action_done_cb(self, controller, terminal_state, result):
self._action_running[controller] = False
def _call_action(self, goals):
for client in self._clients:
self._action_running[client] = True
self._clients[client].send_goal(
goals[client], lambda terminal_state, result: self._action_done_cb(client, terminal_state, result))
def run_joint_trajectory_unsafe(self, joint_trajectory, wait=True):
"""
Moves robot through all joint states with specified timeouts.
@param joint_trajectory - JointTrajectory class object. Represents
trajectory of the joints which would be executed.
@param wait - should method wait for movement end or not.
"""
goals = {}
for controller in self._controllers:
controller_joints = self._controllers[controller]
goal = FollowJointTrajectoryGoal()
goal.trajectory = copy.deepcopy(joint_trajectory)
indices_of_joints_in_this_controller = []
for i, joint in enumerate(joint_trajectory.joint_names):
if joint in controller_joints:
indices_of_joints_in_this_controller.append(i)
goal.trajectory.joint_names = [
joint_trajectory.joint_names[i] for i in indices_of_joints_in_this_controller]
for point in goal.trajectory.points:
if point.positions:
point.positions = [point.positions[i] for i in indices_of_joints_in_this_controller]
if point.velocities:
point.velocities = [point.velocities[i] for i in indices_of_joints_in_this_controller]
if point.effort:
point.effort = [point.effort[i] for i in indices_of_joints_in_this_controller]
goals[controller] = goal
self._call_action(goals)
if not wait:
return
for i in self._clients.keys():
if not self._clients[i].wait_for_result():
rospy.loginfo("Trajectory not completed")
def plan_to_waypoints_target(self, waypoints, reference_frame=None,
eef_step=0.005, jump_threshold=0.0, custom_start_state=None):
"""
Specify a set of waypoints for the end-effector and plans.
This is a blocking method.
@param reference_frame - the reference frame in which the waypoints are given.
@param waypoints - an array of poses of end-effector.
@param eef_step - configurations are computed for every eef_step meters.
@param jump_threshold - maximum distance in configuration space between consecutive points in the
resulting path.
@param custom_start_state - specify a start state different than the current state.
@return - motion plan (RobotTrajectory msg) that contains the trajectory to the set wayapoints targets.
"""
if custom_start_state is None:
self._move_group_commander.set_start_state_to_current_state()
else:
self._move_group_commander.set_start_state(custom_start_state)
old_frame = self._move_group_commander.get_pose_reference_frame()
if reference_frame is not None:
self.set_pose_reference_frame(reference_frame)
self.__plan, fraction = self._move_group_commander.compute_cartesian_path(waypoints, eef_step, jump_threshold)
self.set_pose_reference_frame(old_frame)
return self.__plan, fraction
def set_teach_mode(self, teach):
"""
Activates/deactivates the teach mode for the robot.
Activation: stops the the trajectory controllers for the robot, and
sets it to teach mode.
Deactivation: stops the teach mode and starts trajectory controllers
for the robot.
Currently this method blocks for a few seconds when called on a hand,
while the hand parameters are reloaded.
@param teach - bool to activate or deactivate teach mode
"""
if teach:
mode = RobotTeachModeRequest.TEACH_MODE
else:
mode = RobotTeachModeRequest.TRAJECTORY_MODE
self.change_teach_mode(mode, self._name)
def move_to_trajectory_start(self, trajectory, wait=True):
"""
Make and execute a plan from the current state to the first state in an pre-existing trajectory.
@param trajectory - moveit_msgs/JointTrajectory.
@param wait - Bool to specify if movement should block untill finished.
"""
if len(trajectory.points) <= 0:
rospy.logerr("Trajectory has no points in it, can't reverse...")
return None
first_point = trajectory.points[0]
end_state = dict(zip(trajectory.joint_names, first_point.positions))
self.move_to_joint_value_target(end_state, wait=wait)
@staticmethod
def change_teach_mode(mode, robot):
teach_mode_client = rospy.ServiceProxy('/teach_mode', RobotTeachMode)
req = RobotTeachModeRequest()
req.teach_mode = mode
req.robot = robot
try:
resp = teach_mode_client(req)
if resp.result == RobotTeachModeResponse.ERROR:
rospy.logerr("Failed to change robot %s to mode %d", robot,
mode)
else:
rospy.loginfo("Changed robot %s to mode %d Result = %d", robot,
mode, resp.result)
except rospy.ServiceException:
rospy.logerr("Failed to call service teach_mode")
def get_ik(self, target_pose, avoid_collisions=False, joint_states=None, ik_constraints=None):
"""
Computes the inverse kinematics for a given pose. It returns a JointState.
@param target_pose - A given pose of type PoseStamped.
@param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false.
@param joint_states - initial joint configuration of type JointState from which the IK solution is computed.
If set to None, the current joint state is retrieved automatically.
@param ik_constraints - Set constraints of type Constraints for computing the IK solution.
"""
service_request = PositionIKRequest()
service_request.group_name = self._name
service_request.ik_link_name = self._move_group_commander.get_end_effector_link()
service_request.pose_stamped = target_pose
service_request.timeout.secs = 1
service_request.avoid_collisions = avoid_collisions
if ik_constraints is not None:
service_request.constraints = ik_constraints
if joint_states is None:
service_request.robot_state.joint_state = self.get_joints_state()
else:
service_request.robot_state.joint_state = joint_states
try:
resp = self._compute_ik(ik_request=service_request)
# Check if error_code.val is SUCCESS=1
if resp.error_code.val != 1:
if resp.error_code.val == -10:
rospy.logerr("Unreachable point: Start state in collision")
elif resp.error_code.val == -12:
rospy.logerr("Unreachable point: Goal state in collision")
elif resp.error_code.val == -31:
rospy.logerr("Unreachable point: No IK solution")
else:
rospy.logerr("Unreachable point (error: %s)" % resp.error_code)
return
else:
return resp.solution.joint_state
except rospy.ServiceException as e:
rospy.logerr("Service call failed: %s" % e)
def move_to_pose_value_target_unsafe(self, target_pose, avoid_collisions=False,
time=0.002, wait=True, ik_constraints=None):
"""
Specify a target pose for the end-effector and moves to it.
@param target_pose - new pose of end-effector: a Pose message, a PoseStamped
message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
of 7 floats [x, y, z, qx, qy, qz, qw].
@param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false.
@param time - time in s (counting from now) for the robot to reach the
target (it needs to be greater than 0.0 for it not to be rejected by
the trajectory controller).
@param wait - should method wait for movement end or not.
@param ik_constraints - Set constraints of type Constraints for computing the IK solution.
"""
joint_state = self.get_ik(target_pose, avoid_collisions, ik_constraints=ik_constraints)
if joint_state is not None:
active_joints = self._move_group_commander.get_active_joints()
current_indices = [i for i, x in enumerate(joint_state.name) if any(thing in x for thing in active_joints)]
current_names = [joint_state.name[i] for i in current_indices]
current_positions = [joint_state.position[i] for i in current_indices]
state_as_dict = dict(zip(current_names, current_positions))
self.move_to_joint_value_target_unsafe(state_as_dict, time=time, wait=wait)
class CalibrationMovements:
def __init__(self, move_group_name, max_velocity_scaling, max_acceleration_scaling, angle_delta, translation_delta, move_group_namespace='/'):
# self.client = HandeyeClient() # TODO: move around marker when eye_on_hand, automatically take samples via trigger topic
if not move_group_namespace.endswith('/'):
move_group_namespace = move_group_namespace + '/'
if move_group_namespace != '/':
self.mgc = MoveGroupCommander(move_group_name, robot_description=move_group_namespace+'robot_description', ns=move_group_namespace)
else:
self.mgc = MoveGroupCommander(move_group_name)
self.mgc.set_planner_id("RRTConnectkConfigDefault") # TODO: this is only needed for the UR5
self.mgc.set_max_velocity_scaling_factor(max_velocity_scaling)
self.mgc.set_max_acceleration_scaling_factor(max_acceleration_scaling)
self.start_pose = self.mgc.get_current_pose()
self.joint_limits = [math.radians(90)] * 5 + [math.radians(180)] + [math.radians(350)] # TODO: make param
self.target_poses = []
self.current_pose_index = None
self.current_plan = None
self.fallback_joint_limits = [math.radians(90)] * 4 + [math.radians(90)] + [math.radians(180)] + [
math.radians(350)]
if len(self.mgc.get_active_joints()) == 6:
self.fallback_joint_limits = self.fallback_joint_limits[1:]
self.angle_delta=angle_delta
self.translation_delta=translation_delta
def set_and_check_starting_position(self):
# sets the starting position to the current robot cartesian EE position and checks movement in all directions
# TODO: make repeatable
# - save the home position and each target position as joint position
# - plan to each target position and back, save plan if not crazy
# - return true if can plan to each target position without going crazy
self.start_pose = self.mgc.get_current_pose()
self.target_poses = self._compute_poses_around_state(self.start_pose, self.angle_delta, self.translation_delta)
self.current_pose_index = -1
ret = self._check_target_poses(self.joint_limits)
if ret:
rospy.loginfo("Set current pose as home")
return True
else:
rospy.logerr("Can't calibrate from this position!")
self.start_pose = None
self.target_poses = None
return False
def select_target_pose(self, i):
number_of_target_poses = len(self.target_poses)
if 0 <= i < number_of_target_poses:
rospy.loginfo("Selected pose {} for next movement".format(i))
self.current_pose_index = i
return True
else:
rospy.logerr("Index {} is out of bounds: there are {} target poses".format(i, number_of_target_poses))
return False
def plan_to_start_pose(self):
# TODO: use joint position http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html#planning-to-a-joint-goal
rospy.loginfo("Planning to home pose")
return self._plan_to_pose(self.start_pose)
def plan_to_current_target_pose(self):
# TODO: use joint position http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html#planning-to-a-joint-goal
i = self.current_pose_index
rospy.loginfo("Planning to target pose {}".format(i))
return self._plan_to_pose(self.target_poses[i])
def execute_plan(self):
if self.plan is None:
rospy.logerr("No plan found!")
return False
if CalibrationMovements._is_crazy_plan(self.plan, self.fallback_joint_limits):
rospy.logerr("Crazy plan found, not executing!")
return False
self.mgc.execute(self.plan)
return True
def _plan_to_pose(self, pose):
self.mgc.set_start_state_to_current_state()
self.mgc.set_pose_target(pose)
ret = self.mgc.plan()
if type(ret) is tuple:
# noetic
success, plan, planning_time, error_code = ret
else:
# melodic
plan = ret
if CalibrationMovements._is_crazy_plan(plan, self.fallback_joint_limits):
rospy.logwarn("Planning failed")
self.plan = None
return False
else:
rospy.loginfo("Planning successful")
self.plan = plan
return True
def _check_target_poses(self, joint_limits):
if len(self.fallback_joint_limits) == 6:
joint_limits = joint_limits[1:]
for fp in self.target_poses:
self.mgc.set_pose_target(fp)
ret = self.mgc.plan()
if type(ret) is tuple:
# noetic
success, plan, planning_time, error_code = ret
else:
# melodic
plan = ret
#if len(plan.joint_trajectory.points) == 0 or CalibrationMovements._is_crazy_plan(plan, joint_limits):
#return False
return True
@staticmethod
def _compute_poses_around_state(start_pose, angle_delta, translation_delta):
basis = np.eye(3)
pos_deltas = [quaternion_from_euler(*rot_axis * angle_delta) for rot_axis in basis]
neg_deltas = [quaternion_from_euler(*rot_axis * (-angle_delta)) for rot_axis in basis]
quaternion_deltas = list(chain.from_iterable(zip(pos_deltas, neg_deltas))) # interleave
final_rots = []
for qd in quaternion_deltas:
final_rots.append(list(qd))
# TODO: accept a list of delta values
pos_deltas = [quaternion_from_euler(*rot_axis * angle_delta / 2) for rot_axis in basis]
neg_deltas = [quaternion_from_euler(*rot_axis * (-angle_delta / 2)) for rot_axis in basis]
quaternion_deltas = list(chain.from_iterable(zip(pos_deltas, neg_deltas))) # interleave
for qd in quaternion_deltas:
final_rots.append(list(qd))
final_poses = []
for rot in final_rots:
fp = deepcopy(start_pose)
ori = fp.pose.orientation
combined_rot = quaternion_multiply([ori.x, ori.y, ori.z, ori.w], rot)
fp.pose.orientation = Quaternion(*combined_rot)
final_poses.append(fp)
fp = deepcopy(start_pose)
fp.pose.position.x += translation_delta / 2
final_poses.append(fp)
fp = deepcopy(start_pose)
fp.pose.position.x -= translation_delta / 2
final_poses.append(fp)
fp = deepcopy(start_pose)
fp.pose.position.y += translation_delta
final_poses.append(fp)
fp = deepcopy(start_pose)
fp.pose.position.y -= translation_delta
final_poses.append(fp)
fp = deepcopy(start_pose)
fp.pose.position.z += translation_delta / 3
final_poses.append(fp)
return final_poses
@staticmethod
def _rot_per_joint(plan, degrees=False):
np_traj = np.array([p.positions for p in plan.joint_trajectory.points])
if len(np_traj) == 0:
raise ValueError
np_traj_max_per_joint = np_traj.max(axis=0)
np_traj_min_per_joint = np_traj.min(axis=0)
ret = abs(np_traj_max_per_joint - np_traj_min_per_joint)
if degrees:
ret = [math.degrees(j) for j in ret]
return ret
@staticmethod
def _is_crazy_plan(plan, max_rotation_per_joint):
abs_rot_per_joint = CalibrationMovements._rot_per_joint(plan)
if (abs_rot_per_joint > max_rotation_per_joint).any():
return True
else:
return False
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('TCP_Move', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('xarm6')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('link_base')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.001)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(0.1)
arm.set_max_velocity_scaling_factor(0.1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 角度弧度转换
j1 = 90.0 / 180 * math.pi
j2 = -18.6 / 180 * math.pi
j3 = -28.1 / 180 * math.pi
j4 = 1.0 / 180 * math.pi
j5 = 47.6 / 180 * math.pi
j6 = -0.9 / 180 * math.pi
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）u
joint_positions = [j1, j2, j3, j4, j5, j6]
arm.set_joint_value_target(joint_positions)
arm.go()
rospy.sleep(1)
# 向下按压门把手
current_pose = arm.get_current_joint_values()
current_pose[4] += (20.0 / 180.0) * math.pi
arm.set_joint_value_target(current_pose)
arm.go()
rospy.sleep(1)
# 获取当前位姿数据最为机械臂点对点运动的起始位姿
start_pose = arm.get_current_pose(end_effector_link).pose
# 初始化路点列表
waypoints = [] #放入点的位置
# 设置路点数据，并加入路点列表
wpose = deepcopy(start_pose)
wpose.position.z += 0.053
waypoints.append(deepcopy(wpose)) # z方向上的位移
wpose.position.x += 0.2018
waypoints.append(deepcopy(wpose)) # x方向上的位移
#wpose.position.y -= 0.0
#waypoints.append(deepcopy(wpose)) # y方向上的位移
if cartesian:
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses，路点列表
0.01, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
#rospy.loginfo(str(waypoints) )
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " +
str(fraction) + " success after " +
str(maxtries) + " attempts.")
rospy.sleep(1)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
#initialize the move_group api
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo', anonymous=True)
#scene=PlanningSceneInterface()
#self.scene_pub=rospy.Publisher('planning_scene',PlanningScene)
#self.colors=dict()
arm = MoveGroupCommander('arm')
end_effector_link = arm.get_end_effector_link()
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
arm.allow_replanning(True)
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.01)
arm.set_planning_time(5)
'''
l_tool_size=[0.02,0.02,0.1]
p=PoseStamped()
p.header.frame_id=end_effector_link
p.pose.position.x=-0.0
p.pose.position.y=0.03
p.pose.position.z=0.0
p.pose.orientation.w=1
scene.attach_box(end_effector_link,'l_tool',p,l_tool_size)
r_tool_size=[0.02,0.02,0.1]
p1=PoseStamped()
p1.header.frame_id=end_effector_link
p1.pose.position.x=-0.0
p1.pose.position.y=-0.03
p1.pose.position.z=0.0
p1.pose.orientation.w=1
scene.attach_box(end_effector_link,'r_tool',p1,r_tool_size)
'''
'''
table_id='table'
box1_id='box1'
box2_id='box2'
tool_id='tool'
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(table_id)
scene.remove_attached_object(end_effector_link,tool_id)
rospy.sleep(2)
table_ground=0.36
table_size=[0.5,0.7,0.01]
box1_size=[0.1,0.05,0.03]
box2_size=[0.05,0.05,0.1]
tool_size=[0.2,0.02,0.02]
table_pose=PoseStamped()
table_pose.header.frame_id=reference_frame
table_pose.pose.position.x=0.8
table_pose.pose.position.y=0.0
table_pose.pose.position.z=table_ground+table_size[2]/2.0
table_pose.pose.orientation.w=1.0
scene.add_box(table_id,table_pose,table_size)
box1_pose=PoseStamped()
box1_pose.header.frame_id=reference_frame
box1_pose.pose.position.x=0.6
box1_pose.pose.position.y=-0.2
box1_pose.pose.position.z=table_ground+table_size[2]+box1_size[2]/2.0
box1_pose.pose.orientation.w=1.0
scene.add_box(box1_id,box1_pose,box1_size)
box2_pose=PoseStamped()
box2_pose.header.frame_id=reference_frame
box2_pose.pose.position.x=0.6
box2_pose.pose.position.y=0.0
box2_pose.pose.position.z=table_ground+table_size[2]+box2_size[2]/2.0
box2_pose.pose.orientation.w=1.0
scene.add_box(box2_id,box2_pose,box2_size)
p=PoseStamped()
p.header.frame_id=end_effector_link
p.pose.position.x=-0.06
p.pose.position.y=0.0
p.pose.position.z=0.0
p.pose.orientation.w=1
scene.attach_box(end_effector_link,'tool',p,tool_size)
'''
arm.set_named_target("initial_arm1")
arm.go()
rospy.sleep(5)
target_pose = PoseStamped()
target_pose.header.frame_id = 'base_footprint'
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.4
target_pose.pose.position.y = -0.09
#target_pose.pose.position.z=table_pose.pose.position.z+table_size[2]+0.15
target_pose.pose.position.z = 0.7
target_pose.pose.orientation.x = 0
target_pose.pose.orientation.y = 0
target_pose.pose.orientation.z = 0
target_pose.pose.orientation.w = 1
arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose, end_effector_link)
#arm.go()
traj = arm.plan()
arm.execute(traj)
rospy.sleep(5)
arm.shift_pose_target(1, -0.1, end_effector_link)
arm.go()
arm.shift_pose_target(3, -1.57, end_effector_link)
arm.go()
rospy.sleep(5)
saved_target_pose = arm.get_current_pose(end_effector_link)
arm.set_named_target("initial_arm2")
arm.go()
rospy.sleep(2)
arm.set_pose_target(saved_target_pose, end_effector_link)
arm.go()
rospy.sleep(2)
arm.set_named_target("initial_arm1")
arm.go()
rospy.sleep(2)
#traj=arm.plan()
#arm.execute(traj)
#rospy.sleep(1)
#positions= [0.113, 0.548, 0.767, 0.505, 0, -0.168, -0.197]
#arm.set_joint_value_target(positions)
#arm.go()
#rospy.sleep(1)
#scene.remove_attached_object(end_effector_link,tool_id)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
robot = MoveGroupCommander("arm_1")
rospy.sleep(1)
currentrf = robot.get_pose_reference_frame()
robot.set_pose_reference_frame(currentrf)
newpose = robot.get_current_pose().pose
a = robot.get_current_pose().pose
newposerpy = robot.get_current_rpy()
newpose.position.x = newpose.position.x - delta_x
newpose.position.x = newpose.position.y - delta_y
newpose.position.x = newpose.position.z - delta_z
robot.set_pose_target(newpose)
planned = robot.plan()
robot.execute(planned)
rospy.sleep(2)
newpose = robot.get_current_pose().pose
robot.set_start_state_to_current_state()
robot.set_pose_target(a)
b = robot.compute_cartesian_path([newpose, a], 0.01, 10000)
pathplan = robot.plan(b[0])
fraction = b[1]
robot.execute(pathplan)
finalpose = robot.get_current_pose().pose
rospy.spin()
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
self.colors = dict()
rospy.sleep(1)
arm = MoveGroupCommander('arm')
gripper = MoveGroupCommander('gripper')
end_effector_link = arm.get_end_effector_link()
arm.set_goal_position_tolerance(0.005)
arm.set_goal_orientation_tolerance(0.025)
arm.allow_replanning(True)
gripper.set_goal_position_tolerance(0.005)
gripper.set_goal_orientation_tolerance(0.025)
gripper.allow_replanning(True)
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
arm.set_planning_time(5)
r_tool_size = [0.03, 0.01, 0.06]
l_tool_size = [0.03, 0.01, 0.06]
#left gripper
l_p = PoseStamped()
l_p.header.frame_id = end_effector_link
l_p.pose.position.x = 0.00
l_p.pose.position.y = 0.04
l_p.pose.position.z = 0.04
l_p.pose.orientation.w = 1
scene.attach_box(end_effector_link, 'l_tool', l_p, l_tool_size)
#right gripper
r_p = PoseStamped()
r_p.header.frame_id = end_effector_link
r_p.pose.position.x = 0.00
r_p.pose.position.y = -0.04
r_p.pose.position.z = 0.04
r_p.pose.orientation.w = 1
scene.attach_box(end_effector_link, 'r_tool', r_p, r_tool_size)
grasp_pose = target_pose
grasp_pose.pose.position.x -= 0.15
#grasp_pose.pose.position.z=
grasp_pose.pose.orientation.x = 0
grasp_pose.pose.orientation.y = 0.707
grasp_pose.pose.orientation.z = 0
grasp_pose.pose.orientation.w = 0.707
arm.set_start_state_to_current_state()
arm.set_pose_target(grasp_pose, end_effector_link)
traj = arm.plan()
arm.execute(traj)
rospy.sleep(2)
#arm.shift_pose_target(4,1.57,end_effector_link)
#arm.go()
#rospy.sleep(2)
arm.shift_pose_target(0, 0.11, end_effector_link)
arm.go()
rospy.sleep(2)
saved_target_pose = arm.get_current_pose(end_effector_link)
#arm.set_named_target("initial_arm2")
#grasp
scene.attach_box(end_effector_link, target_id, g_p, target_size)
rospy.sleep(2)
#grasping is over , from now is placing
arm.shift_pose_target(2, 0.15, end_effector_link)
arm.go()
rospy.sleep(2)
arm.shift_pose_target(1, -0.2, end_effector_link)
arm.go()
rospy.sleep(2)
#pouring test
arm.shift_pose_target(3, 1.57, end_effector_link)
rospy.sleep(2)
arm.shift_pose_target(3, -1.57, end_effector_link)
rospy.sleep(2)
#gripper.shift_pose_target(3,1.57,end_effector_link)
arm.set_pose_target(saved_target_pose)
arm.go()
rospy.sleep(2)
scene.remove_attached_object(end_effector_link, 'l_tool')
scene.remove_attached_object(end_effector_link, 'r_tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class SmartGrasper(object):
"""
This is the helper library to easily access the different functionalities of the simulated robot
from python.
"""
__last_joint_state = None
__current_model_name = "cricket_ball"
__path_to_models = "/root/.gazebo/models/"
def __init__(self):
"""
This constructor initialises the different necessary connections to the topics and services
and resets the world to start in a good position.
"""
rospy.init_node("smart_grasper")
self.__joint_state_sub = rospy.Subscriber("/joint_states", JointState,
self.__joint_state_cb, queue_size=1)
rospy.wait_for_service("/gazebo/get_model_state", 10.0)
rospy.wait_for_service("/gazebo/reset_world", 10.0)
self.__reset_world = rospy.ServiceProxy("/gazebo/reset_world", Empty)
self.__get_pose_srv = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState)
rospy.wait_for_service("/gazebo/pause_physics")
self.__pause_physics = rospy.ServiceProxy("/gazebo/pause_physics", Empty)
rospy.wait_for_service("/gazebo/unpause_physics")
self.__unpause_physics = rospy.ServiceProxy("/gazebo/unpause_physics", Empty)
rospy.wait_for_service("/controller_manager/switch_controller")
self.__switch_ctrl = rospy.ServiceProxy("/controller_manager/switch_controller", SwitchController)
rospy.wait_for_service("/gazebo/set_model_configuration")
self.__set_model = rospy.ServiceProxy("/gazebo/set_model_configuration", SetModelConfiguration)
rospy.wait_for_service("/gazebo/delete_model")
self.__delete_model = rospy.ServiceProxy("/gazebo/delete_model", DeleteModel)
rospy.wait_for_service("/gazebo/spawn_sdf_model")
self.__spawn_model = rospy.ServiceProxy("/gazebo/spawn_sdf_model", SpawnModel)
rospy.wait_for_service('/get_planning_scene', 10.0)
self.__get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)
self.__pub_planning_scene = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10, latch=True)
self.arm_commander = MoveGroupCommander("arm")
self.hand_commander = MoveGroupCommander("hand")
self.__hand_traj_client = SimpleActionClient("/hand_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
self.__arm_traj_client = SimpleActionClient("/arm_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
if self.__hand_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False:
rospy.logfatal("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.")
raise Exception("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.")
if self.__arm_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False:
rospy.logfatal("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.")
raise Exception("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.")
self.reset_world()
def reset_world(self):
"""
Resets the object poses in the world and the robot joint angles.
"""
self.__switch_ctrl.call(start_controllers=[],
stop_controllers=["hand_controller", "arm_controller", "joint_state_controller"],
strictness=SwitchControllerRequest.BEST_EFFORT)
self.__pause_physics.call()
joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint', 'H1_F1J1', 'H1_F1J2', 'H1_F1J3',
'H1_F2J1', 'H1_F2J2', 'H1_F2J3', 'H1_F3J1', 'H1_F3J2', 'H1_F3J3']
joint_positions = [1.2, 0.3, -1.5, -0.5, -1.5, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0]
self.__set_model.call(model_name="smart_grasping_sandbox",
urdf_param_name="robot_description",
joint_names=joint_names,
joint_positions=joint_positions)
timer = Timer(0.0, self.__start_ctrl)
timer.start()
time.sleep(0.1)
self.__unpause_physics.call()
self.__reset_world.call()
def get_object_pose(self):
"""
Gets the pose of the ball in the world frame.
@return The pose of the ball.
"""
return self.__get_pose_srv.call(self.__current_model_name, "world").pose
def get_tip_pose(self):
"""
Gets the current pose of the robot's tooltip in the world frame.
@return the tip pose
"""
return self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose
def move_tip_absolute(self, target):
"""
Moves the tooltip to the absolute target in the world frame
@param target is a geometry_msgs.msg.Pose
@return True on success
"""
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([target])
plan = self.arm_commander.plan()
if not self.arm_commander.execute(plan):
return False
return True
def move_tip(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0.):
"""
Moves the tooltip in the world frame by the given x,y,z / roll,pitch,yaw.
@return True on success
"""
transform = PyKDL.Frame(PyKDL.Rotation.RPY(pitch, roll, yaw),
PyKDL.Vector(-x, -y, -z))
tip_pose = self.get_tip_pose()
tip_pose_kdl = posemath.fromMsg(tip_pose)
final_pose = toMsg(tip_pose_kdl * transform)
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([final_pose])
plan = self.arm_commander.plan()
if not self.arm_commander.execute(plan):
return False
return True
def send_command(self, command, duration=0.2):
"""
Send a dictionnary of joint targets to the arm and hand directly.
@param command: a dictionnary of joint names associated with a target:
{"H1_F1J1": -1.0, "shoulder_pan_joint": 1.0}
@param duration: the amount of time it will take to get there in seconds. Needs to be bigger than 0.0
"""
hand_goal = None
arm_goal = None
for joint, target in command.items():
if "H1" in joint:
if not hand_goal:
hand_goal = FollowJointTrajectoryGoal()
point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration.from_sec(duration)
hand_goal.trajectory.points.append(point)
hand_goal.trajectory.joint_names.append(joint)
hand_goal.trajectory.points[0].positions.append(target)
else:
if not arm_goal:
arm_goal = FollowJointTrajectoryGoal()
point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration.from_sec(duration)
arm_goal.trajectory.points.append(point)
arm_goal.trajectory.joint_names.append(joint)
arm_goal.trajectory.points[0].positions.append(target)
if arm_goal:
self.__arm_traj_client.send_goal_and_wait(arm_goal)
if hand_goal:
self.__hand_traj_client.send_goal_and_wait(hand_goal)
def get_current_joint_state(self):
"""
Gets the current state of the robot.
@return joint positions, velocity and efforts as three dictionnaries
"""
joints_position = {n: p for n, p in
zip(self.__last_joint_state.name,
self.__last_joint_state.position)}
joints_velocity = {n: v for n, v in
zip(self.__last_joint_state.name,
self.__last_joint_state.velocity)}
joints_effort = {n: v for n, v in
zip(self.__last_joint_state.name,
self.__last_joint_state.effort)}
return joints_position, joints_velocity, joints_effort
def open_hand(self):
"""
Opens the hand.
@return True on success
"""
self.hand_commander.set_named_target("open")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
return True
def close_hand(self):
"""
Closes the hand.
@return True on success
"""
self.hand_commander.set_named_target("close")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
return True
def check_fingers_collisions(self, enable=True):
"""
Disables or enables the collisions check between the fingers and the objects / table
@param enable: set to True to enable / False to disable
@return True on success
"""
objects = ["cricket_ball__link", "drill__link", "cafe_table__link"]
while self.__pub_planning_scene.get_num_connections() < 1:
rospy.loginfo("waiting for someone to subscribe to the /planning_scene")
rospy.sleep(0.1)
request = PlanningSceneComponents(components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX)
response = self.__get_planning_scene(request)
acm = response.scene.allowed_collision_matrix
for object_name in objects:
if object_name not in acm.entry_names:
# add object to allowed collision matrix
acm.entry_names += [object_name]
for row in range(len(acm.entry_values)):
acm.entry_values[row].enabled += [False]
new_row = deepcopy(acm.entry_values[0])
acm.entry_values += {new_row}
for index_entry_values, entry_values in enumerate(acm.entry_values):
if "H1_F" in acm.entry_names[index_entry_values]:
for index_value, _ in enumerate(entry_values.enabled):
if acm.entry_names[index_value] in objects:
if enable:
acm.entry_values[index_entry_values].enabled[index_value] = False
else:
acm.entry_values[index_entry_values].enabled[index_value] = True
elif acm.entry_names[index_entry_values] in objects:
for index_value, _ in enumerate(entry_values.enabled):
if "H1_F" in acm.entry_names[index_value]:
if enable:
acm.entry_values[index_entry_values].enabled[index_value] = False
else:
acm.entry_values[index_entry_values].enabled[index_value] = True
planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm)
self.__pub_planning_scene.publish(planning_scene_diff)
rospy.sleep(1.0)
return True
def pick(self):
"""
Does its best to pick the ball.
"""
rospy.loginfo("Moving to Pregrasp")
self.open_hand()
time.sleep(0.1)
ball_pose = self.get_object_pose()
ball_pose.position.z += 0.5
#setting an absolute orientation (from the top)
quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0)
ball_pose.orientation.x = quaternion[0]
ball_pose.orientation.y = quaternion[1]
ball_pose.orientation.z = quaternion[2]
ball_pose.orientation.w = quaternion[3]
self.move_tip_absolute(ball_pose)
time.sleep(0.1)
rospy.loginfo("Grasping")
self.move_tip(y=-0.164)
time.sleep(0.1)
self.check_fingers_collisions(False)
time.sleep(0.1)
self.close_hand()
time.sleep(0.1)
rospy.loginfo("Lifting")
for _ in range(5):
self.move_tip(y=0.01)
time.sleep(0.1)
self.check_fingers_collisions(True)
def swap_object(self, new_model_name):
"""
Replaces the current object with a new one.Replaces
@new_model_name the name of the folder in which the object is (e.g. beer)
"""
try:
self.__delete_model(self.__current_model_name)
except:
rospy.logwarn("Failed to delete: " + self.__current_model_name)
try:
sdf = None
initial_pose = Pose()
initial_pose.position.x = 0.15
initial_pose.position.z = 0.82
with open(self.__path_to_models + new_model_name + "/model.sdf", "r") as model:
sdf = model.read()
res = self.__spawn_model(new_model_name, sdf, "", initial_pose, "world")
rospy.logerr( "RES: " + str(res) )
self.__current_model_name = new_model_name
except:
rospy.logwarn("Failed to delete: " + self.__current_model_name)
def __compute_arm_target_for_ball(self):
ball_pose = self.get_object_pose()
# come at it from the top
arm_target = ball_pose
arm_target.position.z += 0.5
quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0)
arm_target.orientation.x = quaternion[0]
arm_target.orientation.y = quaternion[1]
arm_target.orientation.z = quaternion[2]
arm_target.orientation.w = quaternion[3]
return arm_target
def __pre_grasp(self, arm_target):
self.hand_commander.set_named_target("open")
plan = self.hand_commander.plan()
self.hand_commander.execute(plan, wait=True)
for _ in range(10):
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([arm_target])
plan = self.arm_commander.plan()
if self.arm_commander.execute(plan):
return True
def __grasp(self, arm_target):
waypoints = []
waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose)
arm_above_ball = deepcopy(arm_target)
arm_above_ball.position.z -= 0.12
waypoints.append(arm_above_ball)
self.arm_commander.set_start_state_to_current_state()
(plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0)
print fraction
if not self.arm_commander.execute(plan):
return False
self.hand_commander.set_named_target("close")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
self.hand_commander.attach_object("cricket_ball__link")
def __lift(self, arm_target):
waypoints = []
waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose)
arm_above_ball = deepcopy(arm_target)
arm_above_ball.position.z += 0.1
waypoints.append(arm_above_ball)
self.arm_commander.set_start_state_to_current_state()
(plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0)
print fraction
if not self.arm_commander.execute(plan):
return False
def __start_ctrl(self):
rospy.loginfo("STARTING CONTROLLERS")
self.__switch_ctrl.call(start_controllers=["hand_controller", "arm_controller", "joint_state_controller"],
stop_controllers=[], strictness=1)
def __joint_state_cb(self, msg):
self.__last_joint_state = msg
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
cartesian = rospy.get_param('~cartesian', True)
# Connect to the right_arm move group
right_arm = MoveGroupCommander('right_arm')
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame('base_footprint')
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "straight_forward" configuration stored in the SRDF file
right_arm.set_named_target('straight_forward')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
# Get the current pose so we can add it as a waypoint
start_pose = right_arm.get_current_pose(end_effector_link).pose
# Initialize the waypoints list
waypoints = []
# Set the first waypoint to be the starting pose
if cartesian:
# Append the pose to the waypoints list
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
# Set the next waypoint back 0.2 meters and right 0.2 meters
wpose.position.x -= 0.2
wpose.position.y -= 0.2
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
right_arm.set_pose_target(wpose)
right_arm.go()
rospy.sleep(1)
# Set the next waypoint to the right 0.15 meters
wpose.position.x += 0.05
wpose.position.y += 0.15
wpose.position.z -= 0.15
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
right_arm.set_pose_target(wpose)
right_arm.go()
rospy.sleep(1)
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(start_pose))
else:
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(1)
if cartesian:
fraction = 0.0
maxtries = 100
attempts = 0
# Set the internal state to the current state
right_arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = right_arm.compute_cartesian_path (
waypoints, # waypoint poses
0.01, # eef_step
0.0, # jump_threshold
True) # avoid_collisions
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
right_arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")
# Move normally back to the 'resting' position
right_arm.set_named_target('resting')
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
right_arm = MoveGroupCommander("right_arm")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
print right_arm.get_current_pose().pose
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 0.570398010274
wpose.orientation.x = -0.279748061111
wpose.orientation.y = -0.719670670452
wpose.orientation.z = -0.280109368412
wpose.position.y = -0.0349149588619
wpose.position.z = 1.69789257029
wpose.position.x = 0.0117939632591
right_arm.set_pose_target(wpose)
plan1 = right_arm.plan()
right_arm.execute(plan1)
print "============ Waiting while RVIZ executes plan1..."
rospy.sleep(5)
waypoints = []
waypoints.append(right_arm.get_current_pose().pose)
print right_arm.get_current_pose().pose
gain = 0.05;
r = 0.05
a = 0.0117939632591 # waypoints[0].position.x
b = 1.64789257 # waypoints[0].position.z
#waypoints[0].position.x = a + r
points = 10
for i in xrange(points):
wpose = geometry_msgs.msg.Pose()
start_pose.orientation.x = 0.393751611087
start_pose.orientation.y = 0.918424640162
start_pose.orientation.z = -0.0150455838492
start_pose.orientation.w = 0.0350639347048
# start_pose.orientation.w = 0
# start_pose.orientation.x = 0
# start_pose.orientation.y = 1
# start_pose.orientation.z = 0
# start_pose.position.y = 0.0256415233819
# start_pose.position.z = 1.25871460368
# start_pose.position.x = 0.243500142238
right_arm.set_pose_target(start_pose)
plan_start = right_arm.plan()
print "============ Waiting while RVIZ displays plan_start..."
rospy.sleep(5)
right_arm.execute(plan_start)
print "============ Waiting while RVIZ executes plan_start..."
rospy.sleep(5)
waypoints = []
waypoints.append(right_arm.get_current_pose().pose)
gain = 0.2
points = 5
for i in xrange(points):
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = waypoints[i-1].orientation.w
wpose.orientation.x = waypoints[i-1].orientation.x
wpose.orientation.y = waypoints[i-1].orientation.y
wpose.orientation.z = waypoints[i-1].orientation.z
wpose.position.y = waypoints[i-1].position.y - 0.0737309
right_arm = MoveGroupCommander("right_arm")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
print right_arm.get_current_pose().pose
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 0.409003402335
wpose.orientation.x = -0.330487715391
wpose.orientation.y = -0.782324118504
wpose.orientation.z = -0.333860839963
wpose.position.y = 0.0776977107894
wpose.position.z = 1.25950497489
wpose.position.x = -0.199191527741
right_arm.set_pose_target(wpose)
plan1 = right_arm.plan()
right_arm.execute(plan1)
print "============ Waiting while RVIZ executes plan1..."
rospy.sleep(5)
waypoints = []
waypoints.append(right_arm.get_current_pose().pose)
print right_arm.get_current_pose().pose
points = 20
for i in xrange(points):
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = waypoints[i-1].orientation.w + 0.428513
wpose.orientation.x = waypoints[i-1].orientation.x
wpose.orientation.y = waypoints[i-1].orientation.y
wpose.orientation.z = waypoints[i-1].orientation.z - 0.175202
wpose.position.y = waypoints[i-1].position.y - 0.025
def cartesian_move_to(pose_, execute=False):
goal_pose = deepcopy(pose_)
######################################## experiment start
# for hardcoded aruco pose
# some known arm positions
# aruco pose in simulation: 0.52680940312, -0.0490591337742, 0.921301848054, 0.504516550338, 0.506271228009, 0.497290765692, 0.49178693403
# in base_footprint: 0.52680940312, -0.0490591337742, 0.871301848054, 0, 0, 0, 0
# pick pose: 0.52680940312, -0.0490591337742, 0.871301848054, 0, 0, 0, 0
# final hand pose in simulation: 0.47653800831, -0.396454309047, 1.05656705145, -0.630265833017, -0.318648344327, 0.582106845777, 0.402963810396
# goal_pose = PoseStamped()
# goal_pose.header.frame_id = 'base_footprint'
# goal_pose.pose.position.x = 0.52680940312
# goal_pose.pose.position.y = -0.0490591337742
# goal_pose.pose.position.z = 0.871301848054
# goal_pose.pose.orientation.x = 0
# goal_pose.pose.orientation.y = 0
# goal_pose.pose.orientation.z = 0
# goal_pose.pose.orientation.w = 1
arm = MoveGroupCommander('arm')
arm.allow_replanning(True)
end_effector_link = arm.get_end_effector_link()
arm.set_goal_position_tolerance(0.03)
arm.set_goal_orientation_tolerance(0.025)
arm.allow_replanning(True)
reference_frame = 'base_footprint'
arm.set_pose_reference_frame(reference_frame)
arm.set_planning_time(5)
rospy.sleep(2)
start_pose = arm.get_current_pose(end_effector_link).pose
rospy.loginfo("End effector start pose: " + str(start_pose))
rospy.loginfo("Aruco pose: " + str(goal_pose))
waypoints = []
waypoints.append(start_pose)
# goal_pose.pose.orientation.x = -0.5
# goal_pose.pose.orientation.y = -0.5
# goal_pose.pose.orientation.z = -0.5
# goal_pose.pose.orientation.w = 1
# goal_pose.pose.orientation.y -= 0.1*(1.0/2.0)
waypoints.append(deepcopy(goal_pose.pose))
fraction = 0.0
maxtries = 100
attempts = 0
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(waypoints, 0.02, 0.0,
True)
attempts += 1
if attempts % 20 == 0:
if (attempts < 100):
rospy.loginfo("still trying after" + str(attempts) +
" attempts...")
else:
rospy.loginfo("Finished after " + str(attempts) +
" attempts...")
if fraction > 0.89:
rospy.loginfo("path compute successfully. Arm is moving.")
print(
plan.joint_trajectory.points[len(plan.joint_trajectory.points)
- 1].positions)
print(plan)
#for item in plan.joint_trajectory.points[len(plan.joint_trajectory.points)-1]
if execute:
arm.execute(plan)
rospy.loginfo("path execution complete. ")
rospy.sleep(2)
if (attempts % 100 == 0 and fraction < 0.9):
rospy.loginfo("path planning failed with only " +
str(fraction * 100) + "% success after " +
str(maxtries) + " attempts")
return fraction
class SrRobotCommander(object):
"""
Base class for hand and arm commanders
"""
__group_prefixes = {"right_arm": "ra_",
"left_arm": "la_",
"right_hand": "rh_",
"left_hand": "lh_"}
def __init__(self, name, prefix=None):
"""
Initialize MoveGroupCommander object
@param name - name of the MoveIt group
"""
self._name = name
self._move_group_commander = MoveGroupCommander(name)
self._robot_commander = RobotCommander()
self._robot_name = self._robot_commander._r.get_robot_name()
self.refresh_named_targets()
self._warehouse_name_get_srv = rospy.ServiceProxy("get_robot_state",
GetState)
self._planning_scene = PlanningSceneInterface()
self._joint_states_lock = threading.Lock()
self._joint_states_listener = \
rospy.Subscriber("joint_states", JointState,
self._joint_states_callback, queue_size=1)
self._joints_position = {}
self._joints_velocity = {}
self._joints_effort = {}
self.__plan = None
rospy.wait_for_service('compute_ik')
self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK)
# prefix of the trajectory controller
if prefix is not None:
self._prefix = prefix
elif name in self.__group_prefixes.keys():
self._prefix = self.__group_prefixes[name]
else:
# Group name is one of the ones to plan for specific fingers.
# We need to find the hand prefix using the hand finder
hand_finder = HandFinder()
hand_parameters = hand_finder.get_hand_parameters()
hand_serial = hand_parameters.mapping.keys()[0]
self._prefix = hand_parameters.joint_prefix[hand_serial]
self._set_up_action_client()
threading.Thread(None, rospy.spin)
def set_planner_id(self, planner_id):
self._move_group_commander.set_planner_id(planner_id)
def set_num_planning_attempts(self, num_planning_attempts):
self._move_group_commander.set_num_planning_attempts(num_planning_attempts)
def set_planning_time(self, seconds):
self._move_group_commander.set_planning_time(seconds)
def get_end_effector_pose_from_named_state(self, name):
state = self._warehouse_name_get_srv(name, self._robot_name).state
return self.get_end_effector_pose_from_state(state)
def get_end_effector_pose_from_state(self, state):
header = Header()
fk_link_names = [self._move_group_commander.get_end_effector_link()]
header.frame_id = self._move_group_commander.get_pose_reference_frame()
response = self._forward_k(header, fk_link_names, state)
return response.pose_stamped[0]
def get_planning_frame(self):
return self._move_group_commander.get_planning_frame()
def set_pose_reference_frame(self, reference_frame):
self._move_group_commander.set_pose_reference_frame(reference_frame)
def get_group_name(self):
return self._name
def refresh_named_targets(self):
self._srdf_names = self.__get_srdf_names()
self._warehouse_names = self.__get_warehouse_names()
def execute(self):
"""
Executes the last plan made.
"""
if self.__plan is not None:
self._move_group_commander.execute(self.__plan)
self.__plan = None
else:
rospy.logwarn("No plans where made, not executing anything.")
def move_to_joint_value_target(self, joint_states, wait=True,
angle_degrees=False):
"""
Set target of the robot's links and moves to it.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param wait - should method wait for movement end or not
@param angle_degrees - are joint_states in degrees or not
"""
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update((joint, radians(i))
for joint, i in joint_states_cpy.items())
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_joint_value_target(joint_states_cpy)
self._move_group_commander.go(wait=wait)
def plan_to_joint_value_target(self, joint_states, angle_degrees=False):
"""
Set target of the robot's links and plans.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param angle_degrees - are joint_states in degrees or not
This is a blocking method.
"""
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update((joint, radians(i))
for joint, i in joint_states_cpy.items())
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_joint_value_target(joint_states_cpy)
self.__plan = self._move_group_commander.plan()
def check_plan_is_valid(self):
"""
Checks if current plan contains a valid trajectory
"""
return (self.__plan is not None and len(self.__plan.joint_trajectory.points) > 0)
def get_robot_name(self):
return self._robot_name
def named_target_in_srdf(self, name):
return name in self._srdf_names
def set_named_target(self, name):
if name in self._srdf_names:
self._move_group_commander.set_named_target(name)
elif (name in self._warehouse_names):
response = self._warehouse_name_get_srv(name, self._robot_name)
active_names = self._move_group_commander._g.get_active_joints()
joints = response.state.joint_state.name
positions = response.state.joint_state.position
js = {}
for n, this_name in enumerate(joints):
if this_name in active_names:
js[this_name] = positions[n]
self._move_group_commander.set_joint_value_target(js)
else:
rospy.logerr("Unknown named state '%s'..." % name)
return False
return True
def get_named_target_joint_values(self, name):
output = dict()
if (name in self._srdf_names):
output = self._move_group_commander.\
_g.get_named_target_values(str(name))
elif (name in self._warehouse_names):
js = self._warehouse_name_get_srv(
name, self._robot_name).state.joint_state
for x, n in enumerate(js.name):
if n in self._move_group_commander._g.get_joints():
output[n] = js.position[x]
else:
rospy.logerr("No target named %s" % name)
return None
return output
def get_end_effector_link(self):
return self._move_group_commander.get_end_effector_link()
def get_current_pose(self, reference_frame=None):
"""
Get the current pose of the end effector.
@param reference_frame - The desired reference frame in which end effector pose should be returned.
If none is passed, it will use the planning frame as reference.
@return geometry_msgs.msg.Pose() - current pose of the end effector
"""
if reference_frame is not None:
listener = tf.TransformListener()
try:
listener.waitForTransform(reference_frame, self._move_group_commander.get_end_effector_link(),
rospy.Time(0), rospy.Duration(5.0))
(trans, rot) = listener.lookupTransform(reference_frame,
self._move_group_commander.get_end_effector_link(),
rospy.Time(0))
current_pose = geometry_msgs.msg.Pose()
current_pose.position.x = trans[0]
current_pose.position.y = trans[1]
current_pose.position.z = trans[2]
current_pose.orientation.x = rot[0]
current_pose.orientation.y = rot[1]
current_pose.orientation.z = rot[2]
current_pose.orientation.w = rot[3]
return current_pose
except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logwarn("Couldn't get the pose from " + self._move_group_commander.get_end_effector_link() +
" in " + reference_frame + " reference frame")
return None
else:
return self._move_group_commander.get_current_pose().pose
def get_current_state(self):
"""
Get the current joint state of the group being used.
@return a dictionary with the joint names as keys and current joint values
"""
joint_names = self._move_group_commander._g.get_active_joints()
joint_values = self._move_group_commander._g.get_current_joint_values()
return dict(zip(joint_names, joint_values))
def get_current_state_bounded(self):
"""
Get the current joint state of the group being used, enforcing that they are within each joint limits.
@return a dictionary with the joint names as keys and current joint values
"""
current = self._move_group_commander._g.get_current_state_bounded()
names = self._move_group_commander._g.get_active_joints()
output = {n: current[n] for n in names if n in current}
return output
def get_robot_state_bounded(self):
return self._move_group_commander._g.get_current_state_bounded()
def move_to_named_target(self, name, wait=True):
"""
Set target of the robot's links and moves to it
@param name - name of the target pose defined in SRDF
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
if self.set_named_target(name):
self._move_group_commander.go(wait=wait)
def plan_to_named_target(self, name):
"""
Set target of the robot's links and plans
This is a blocking method.
@param name - name of the target pose defined in SRDF
"""
self._move_group_commander.set_start_state_to_current_state()
if self.set_named_target(name):
self.__plan = self._move_group_commander.plan()
def __get_warehouse_names(self):
try:
list_srv = rospy.ServiceProxy("list_robot_states", ListStates)
return list_srv("", self._robot_name).states
except rospy.ServiceException as exc:
rospy.logwarn("Couldn't access warehouse: " + str(exc))
return list()
def _reset_plan(self):
self.__plan = None
def _set_plan(self, plan):
self.__plan = plan
def __get_srdf_names(self):
return self._move_group_commander._g.get_named_targets()
def get_named_targets(self):
"""
Get the complete list of named targets, from SRDF
as well as warehouse poses if available.
@return list of strings containing names of targets.
"""
return self._srdf_names + self._warehouse_names
def get_joints_position(self):
"""
Returns joints position
@return - dictionary with joints positions
"""
with self._joint_states_lock:
return self._joints_position
def get_joints_velocity(self):
"""
Returns joints velocities
@return - dictionary with joints velocities
"""
with self._joint_states_lock:
return self._joints_velocity
def _get_joints_effort(self):
"""
Returns joints effort
@return - dictionary with joints efforts
"""
with self._joint_states_lock:
return self._joints_effort
def run_joint_trajectory(self, joint_trajectory):
"""
Moves robot through all joint states with specified timeouts
@param joint_trajectory - JointTrajectory class object. Represents
trajectory of the joints which would be executed.
"""
plan = RobotTrajectory()
plan.joint_trajectory = joint_trajectory
self._move_group_commander.execute(plan)
def make_named_trajectory(self, trajectory):
"""
Makes joint value trajectory from specified by named poses (either from
SRDF or from warehouse)
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements (n.b either name or joint_angles is required)
- name -> the name of the way point
- joint_angles -> a dict of joint names and angles
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
- degrees -> set to true if joint_angles is specified in degrees. Assumed false if absent.
"""
current = self.get_current_state_bounded()
joint_trajectory = JointTrajectory()
joint_names = current.keys()
joint_trajectory.joint_names = joint_names
time_from_start = 0.0
for wp in trajectory:
joint_positions = None
if 'name' in wp.keys():
joint_positions = self.get_named_target_joint_values(wp['name'])
elif 'joint_angles' in wp.keys():
joint_positions = copy.deepcopy(wp['joint_angles'])
if 'degrees' in wp.keys() and wp['degrees']:
for joint, angle in joint_positions.iteritems():
joint_positions[joint] = radians(angle)
if joint_positions is None:
rospy.logerr("Invalid waypoint. Must contain valid name for named target or dict of joint angles.")
return None
new_positions = {}
for n in joint_names:
new_positions[n] = joint_positions[n] if n in joint_positions else current[n]
trajectory_point = JointTrajectoryPoint()
trajectory_point.positions = [new_positions[n] for n in joint_names]
current = new_positions
time_from_start += wp['interpolate_time']
trajectory_point.time_from_start = rospy.Duration.from_sec(time_from_start)
joint_trajectory.points.append(trajectory_point)
if 'pause_time' in wp and wp['pause_time'] > 0:
extra = JointTrajectoryPoint()
extra.positions = trajectory_point.positions
time_from_start += wp['pause_time']
extra.time_from_start = rospy.Duration.from_sec(time_from_start)
joint_trajectory.points.append(extra)
return joint_trajectory
def send_stop_trajectory_unsafe(self):
"""
Sends a trajectory of all active joints at their current position.
This stops the robot.
"""
current = self.get_current_state_bounded()
trajectory_point = JointTrajectoryPoint()
trajectory_point.positions = current.values()
trajectory_point.time_from_start = rospy.Duration.from_sec(0.1)
trajectory = JointTrajectory()
trajectory.points.append(trajectory_point)
trajectory.joint_names = current.keys()
self.run_joint_trajectory_unsafe(trajectory)
def run_named_trajectory_unsafe(self, trajectory, wait=False):
"""
Moves robot through trajectory specified by named poses, either from
SRDF or from warehouse. Runs trajectory directly via contoller.
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements:
- name -> the name of the way point
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
"""
joint_trajectory = self.make_named_trajectory(trajectory)
if joint_trajectory is not None:
self.run_joint_trajectory_unsafe(joint_trajectory, wait)
def run_named_trajectory(self, trajectory):
"""
Moves robot through trajectory specified by named poses, either from
SRDF or from warehouse. Runs trajectory via moveit.
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements:
- name -> the name of the way point
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
"""
joint_trajectory = self.make_named_trajectory(trajectory)
if joint_trajectory is not None:
self.run_joint_trajectory(joint_trajectory)
def move_to_position_target(self, xyz, end_effector_link="", wait=True):
"""
Specify a target position for the end-effector and moves to it
@param xyz - new position of end-effector
@param end_effector_link - name of the end effector link
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_position_target(xyz, end_effector_link)
self._move_group_commander.go(wait=wait)
def plan_to_position_target(self, xyz, end_effector_link=""):
"""
Specify a target position for the end-effector and plans.
This is a blocking method.
@param xyz - new position of end-effector
@param end_effector_link - name of the end effector link
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_position_target(xyz, end_effector_link)
self.__plan = self._move_group_commander.plan()
def move_to_pose_target(self, pose, end_effector_link="", wait=True):
"""
Specify a target pose for the end-effector and moves to it
@param pose - new pose of end-effector: a Pose message, a PoseStamped
message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
of 7 floats [x, y, z, qx, qy, qz, qw]
@param end_effector_link - name of the end effector link
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_pose_target(pose, end_effector_link)
self._move_group_commander.go(wait=wait)
def plan_to_pose_target(self, pose, end_effector_link=""):
"""
Specify a target pose for the end-effector and plans.
This is a blocking method.
@param pose - new pose of end-effector: a Pose message, a PoseStamped
message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
of 7 floats [x, y, z, qx, qy, qz, qw]
@param end_effector_link - name of the end effector link
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_pose_target(pose, end_effector_link)
self.__plan = self._move_group_commander.plan()
def _joint_states_callback(self, joint_state):
"""
The callback function for the topic joint_states.
It will store the received joint position, velocity and efforts
information into dictionaries
@param joint_state - the message containing the joints data.
"""
with self._joint_states_lock:
self._joints_position = {n: p for n, p in
zip(joint_state.name,
joint_state.position)}
self._joints_velocity = {n: v for n, v in
zip(joint_state.name,
joint_state.velocity)}
self._joints_effort = {n: v for n, v in
zip(joint_state.name, joint_state.effort)}
def _get_trajectory_controller_name(self):
return self._prefix + "trajectory_controller"
def _set_up_action_client(self):
"""
Sets up an action client to communicate with the trajectory controller
"""
self._action_running = False
self._client = SimpleActionClient(
self._get_trajectory_controller_name() + "/follow_joint_trajectory",
FollowJointTrajectoryAction
)
if self._client.wait_for_server(timeout=rospy.Duration(4)) is False:
rospy.logfatal("Failed to connect to action server in 4 sec")
raise Exception("Failed to connect to action server in 4 sec")
def move_to_joint_value_target_unsafe(self, joint_states, time=0.002,
wait=True, angle_degrees=False):
"""
Set target of the robot's links and moves to it.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param time - time in s (counting from now) for the robot to reach the
target (it needs to be greater than 0.0 for it not to be rejected by
the trajectory controller)
@param wait - should method wait for movement end or not
@param angle_degrees - are joint_states in degrees or not
"""
# self._update_default_trajectory()
# self._set_targets_to_default_trajectory(joint_states)
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update((joint, radians(i))
for joint, i in joint_states_cpy.items())
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = list(joint_states_cpy.keys())
point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration.from_sec(time)
for key in goal.trajectory.joint_names:
point.positions.append(joint_states_cpy[key])
goal.trajectory.points = []
goal.trajectory.points.append(point)
self._call_action(goal)
if not wait:
return
if not self._client.wait_for_result():
rospy.loginfo("Trajectory not completed")
def action_is_running(self):
return self._action_running
def _action_done_cb(self, terminal_state, result):
self._action_running = False
def _call_action(self, goal):
self._set_up_action_client()
self._action_running = True
self._client.send_goal(goal, self._action_done_cb)
def run_joint_trajectory_unsafe(self, joint_trajectory, wait=True):
"""
Moves robot through all joint states with specified timeouts
@param joint_trajectory - JointTrajectory class object. Represents
trajectory of the joints which would be executed.
@param wait - should method wait for movement end or not
"""
goal = FollowJointTrajectoryGoal()
goal.trajectory = joint_trajectory
self._call_action(goal)
if not wait:
return
if not self._client.wait_for_result():
rospy.loginfo("Trajectory not completed")
def plan_to_waypoints_target(self, waypoints, reference_frame=None, eef_step=0.005, jump_threshold=0.0):
"""
Specify a set of waypoints for the end-effector and plans.
This is a blocking method.
@param reference_frame - the reference frame in which the waypoints are given
@param waypoints - an array of poses of end-effector
@param eef_step - configurations are computed for every eef_step meters
@param jump_threshold - maximum distance in configuration space between consecutive points in the resulting path
"""
old_frame = self._move_group_commander.get_pose_reference_frame()
if reference_frame is not None:
self.set_pose_reference_frame(reference_frame)
(self.__plan, fraction) = self._move_group_commander.compute_cartesian_path(waypoints, eef_step, jump_threshold)
self.set_pose_reference_frame(old_frame)
def set_teach_mode(self, teach):
"""
Activates/deactivates the teach mode for the robot.
Activation: stops the the trajectory controllers for the robot, and
sets it to teach mode.
Deactivation: stops the teach mode and starts trajectory controllers
for the robot.
Currently this method blocks for a few seconds when called on a hand,
while the hand parameters are reloaded.
@param teach - bool to activate or deactivate teach mode
"""
if teach:
mode = RobotTeachModeRequest.TEACH_MODE
else:
mode = RobotTeachModeRequest.TRAJECTORY_MODE
self.change_teach_mode(mode, self._name)
def move_to_trajectory_start(self, trajectory, wait=True):
"""
Make and execute a plan from the current state to the first state in an pre-existing trajectory
@param trajectory - moveit_msgs/JointTrajectory
@param wait - Bool to specify if movement should block untill finished.
"""
if len(trajectory.points) <= 0:
rospy.logerr("Trajectory has no points in it, can't reverse...")
return None
first_point = trajectory.points[0]
end_state = dict(zip(trajectory.joint_names, first_point.positions))
self.move_to_joint_value_target(end_state, wait=wait)
@staticmethod
def change_teach_mode(mode, robot):
teach_mode_client = rospy.ServiceProxy('/teach_mode', RobotTeachMode)
req = RobotTeachModeRequest()
req.teach_mode = mode
req.robot = robot
try:
resp = teach_mode_client(req)
if resp.result == RobotTeachModeResponse.ERROR:
rospy.logerr("Failed to change robot %s to mode %d", robot,
mode)
else:
rospy.loginfo("Changed robot %s to mode %d Result = %d", robot,
mode, resp.result)
except rospy.ServiceException:
rospy.logerr("Failed to call service teach_mode")
def get_ik(self, target_pose, avoid_collisions=False):
"""
Computes the inverse kinematics for a given pose. It returns a JointState
@param target_pose - A given pose of type PoseStamped
@param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false
"""
service_request = PositionIKRequest()
service_request.group_name = self._name
service_request.ik_link_name = self._move_group_commander.get_end_effector_link()
service_request.pose_stamped = target_pose
service_request.timeout.secs = 0.005
service_request.avoid_collisions = avoid_collisions
try:
resp = self._compute_ik(ik_request=service_request)
# Check if error_code.val is SUCCESS=1
if resp.error_code.val != 1:
if resp.error_code.val == -10:
rospy.logerr("Unreachable point: Start state in collision")
elif resp.error_code.val == -12:
rospy.logerr("Unreachable point: Goal state in collision")
elif resp.error_code.val == -31:
rospy.logerr("Unreachable point: No IK solution")
else:
rospy.logerr("Unreachable point (error: %s)" % resp.error_code)
return
else:
return resp.solution.joint_state
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('draw_move', anonymous=True)
listener = tf.TransformListener()
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.Pose_goal = PoseStamped()
self.target_goal = PoseStamped()
self.next_pose_goal = PoseStamped()
self.theta = 0
self.waypoints = []
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
print("====== move plan go to initial pose ======")
self.Pose_goal.header.frame_id = 'world'
self.Pose_goal.pose.position.x = 0.150927439711 # red line 0.2 0.2
self.Pose_goal.pose.position.y = -0.901780911637 # green line 0.15 0.15
self.Pose_goal.pose.position.z = 0.998914536823 # blue line # 0.35 0.6
# Pose_goal.pose.orientation = start_pose.orientation
self.Pose_goal.pose.orientation.x = 0.681875262636
self.Pose_goal.pose.orientation.y = -0.728650631017
self.Pose_goal.pose.orientation.z = 0.0513402005789
self.Pose_goal.pose.orientation.w = 0.0384521511616
self.robot_arm.set_pose_target(self.Pose_goal,'righttool0')
self.robot_arm.go()
rospy.sleep(5)
self.target_goal.header.frame_id = 'world'
self.target_goal.pose.position.x = 0.150927439711 # red line 0.2 0.2
self.target_goal.pose.position.y = -0.901780911637 # green line 0.15 0.15
self.target_goal.pose.position.z = 0.969077397768 # blue line # 0.35 0.6
# Pose_goal.pose.orientation = start_pose.orientation
self.target_goal.pose.orientation.x = 0.681875262636
self.target_goal.pose.orientation.y = -0.728650631017
self.target_goal.pose.orientation.z = 0.0513402005789
self.target_goal.pose.orientation.w = 0.0384521511616
self.centerA = self.target_goal.pose.position.x
self.centerB = self.target_goal.pose.position.y
while self.theta < 2*pi:
wpose = copy.deepcopy(self.target_goal.pose)
wpose.position.x = self.centerA + 0.05*math.cos(self.theta)
wpose.position.y = self.centerB + 0.05*math.sin(self.theta)
self.waypoints.append(copy.deepcopy(wpose))
self.theta+= 0.1
def move_code(self):
# plan the movement
fraction = 0
maxtries = 100
attempts = 0
self.robot_arm.set_start_state_to_current_state()
while fraction<1.0 and attempts<maxtries:
(plan,fraction)=self.robot_arm.compute_cartesian_path(
self.waypoints,
0.01,
0,
True
)
attempts+=1
print("attempt:%s " %attempts)
# execute the plan
if fraction == 1.0:
self.robot_arm.execute(plan)
else:
print("=========executing fail==========")
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose',
PoseStamped,
queue_size=10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(5)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
arm.set_named_target('up')
arm.go()
# Open the gripper to the neutral position
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Set the height of the table off the ground
table_ground = 0.45
# Set the dimensions of the scene objects [l, w, h]
table_size = [0.2, 1.0, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = -0.45
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = -0.41
box1_pose.pose.position.y = -0.3
box1_pose.pose.position.z = table_ground + table_size[
2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = -0.39
box2_pose.pose.position.y = 0.3
box2_pose.pose.position.z = table_ground + table_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
arm.set_support_surface_name(table_id)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = -0.5
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_ground + table_size[2] + 0.3
target_pose.pose.orientation.w = 1.0
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.5
place_pose.pose.position.y = 0.5
place_pose.pose.position.z = 0
place_pose.pose.orientation.w = 1.0
# Set the start state to the current state
arm.set_start_state_to_current_state()
rospy.sleep(2)
# Set the goal pose of the end effector to the stored pose
arm.set_pose_target(target_pose, end_effector_link)
# Plan the trajectory to the goal
traj = arm.plan()
# Scale the trajectory speed by a factor of 0.25
new_traj = scale_trajectory_speed(traj, 0.25)
# Execute the planned trajectory
arm.execute(new_traj)
rospy.sleep(2)
# Close the gripper to the neutral position
gripper.set_joint_value_target(GRIPPER_CLOSED)
gripper.go()
# Pause for a second
rospy.sleep(2)
# Set the place pose of the end effector to the stored pose
arm.set_pose_target(place_pose, end_effector_link)
arm.go()
rospy.sleep(2)
# Open the gripper to the neutral position
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
arm.set_named_target('resting')
arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class MoveitMove(EventState):
'''
Move the arm to a specific pose or point or named_target
-- move wetter or not the arm should move or simply check if the move is possible
-- waitForExecution wait for execution to end before continuing
># pose Pose Targetpose.
<= done No error occurred.
<= failed The plan could't be done.
'''
def __init__(self,
move=True,
waitForExecution=True,
group="RightArm",
watchdog=15):
# See example_state.py for basic explanations.
super(MoveitMove, self).__init__(outcomes=['done', 'failed'],
input_keys=['target'])
self.move = move
self.waitForExecution = waitForExecution
self.group = MoveGroupCommander(group)
self.tol = 0.06
self.result = None
self.count = 0
self.timeout = rospy.Time.now()
self.watchdog = watchdog
def execute(self, userdata):
if self.result:
return self.result
if self.waitForExecution:
curState = self.group.get_current_joint_values()
diff = compareStates(curState, self.endState)
print("diff=" + str(diff))
if diff < self.tol or self.timeout + rospy.Duration(
self.watchdog) < rospy.Time.now():
self.count += 1
if self.count > 3:
Logger.loginfo('Target reached :)')
return "done"
else:
self.count = 0
else:
return "done"
def on_enter(self, userdata):
Logger.loginfo('Enter Move Arm')
self.timeout = rospy.Time.now()
if type(userdata.target) is Pose:
Logger.loginfo('the target is a pose')
self.group.set_pose_target(userdata.target)
elif type(userdata.target) is Point:
Logger.loginfo('the target is a point')
xyz = [userdata.target.x, userdata.target.y, userdata.target.z]
self.group.set_position_target(xyz)
elif type(userdata.target) is str:
Logger.loginfo('the target is a named_target')
self.group.set_named_target(userdata.target)
else:
Logger.loginfo(
'ERROR in ' + str(self.name) +
' : target is not a Pose() nor a Point() nor a string')
self.result = 'failed'
Logger.loginfo('target defined')
try:
plan = self.group.plan()
Logger.loginfo(str(plan))
# Logger.loginfo(str(plan.joint_trajectory.points))
self.endState = plan.joint_trajectory.points[
len(plan.joint_trajectory.points) - 1].positions
Logger.loginfo(str(self.endState))
except:
Logger.loginfo('Planning failed')
self.result = 'failed'
return
Logger.loginfo('Plan done successfully')
if self.move:
Logger.loginfo('Initiating movement')
self.group.execute(plan, wait=False)
if not self.waitForExecution:
self.result = "done"
else:
Logger.loginfo('The target is reachable')
self.result = "done"
def on_exit(self, userdata):
Logger.loginfo('Stoping movement')
self.group.stop()
def on_pause(self):
Logger.loginfo('Pausing movement')
self.group.stop()
def on_resume(self, userdata):
self.on_enter(userdata)
moveit_msgs.msg.DisplayTrajectory,
queue_size=1)
print right_arm.get_current_pose().pose
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 0.570398010274
wpose.orientation.x = -0.279748061111
wpose.orientation.y = -0.719670670452
wpose.orientation.z = -0.280109368412
wpose.position.y = -0.0349149588619
wpose.position.z = 1.69789257029
wpose.position.x = 0.0117939632591
right_arm.set_pose_target(wpose)
plan1 = right_arm.plan()
right_arm.execute(plan1)
print "============ Waiting while RVIZ executes plan1..."
rospy.sleep(5)
waypoints = []
waypoints.append(right_arm.get_current_pose().pose)
print right_arm.get_current_pose().pose
gain = 0.05
r = 0.05
a = 0.0117939632591 # waypoints[0].position.x
b = 1.64789257 # waypoints[0].position.z
#waypoints[0].position.x = a + r
points = 10
for i in xrange(points):
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = waypoints[i - 1].orientation.w
class MoveitMoveCartesian(EventState):
'''
Move the arm between two specific poses or points or named_targets
-- move Bool Whether or not the arm should move or simply check if the move is possible
-- waitForExecution Bool Wait for execution to end before continuing
-- group string Name of the Moveit Planning group
># targetPose Pose Pose to reach
<= done No error occurred.
<= failed The plan could't be done.
'''
def __init__(self,
move=True,
waitForExecution=True,
group="RightArm",
watchdog=15):
# See example_state.py for basic explanations.
super(MoveitMoveCartesian, self).__init__(outcomes=['done', 'failed'],
input_keys=['targetPose'])
self.move = move
self.waitForExecution = waitForExecution
self.group = MoveGroupCommander(group)
self.tol = 0.06
self.result = None
self.count = 0
self.countlimit = 0
self.movSteps = 0.01
self.jumpThresh = 0.0
self.timeout = rospy.Time.now()
self.watchdog = watchdog
def execute(self, userdata):
if self.result:
return self.result
if self.waitForExecution:
curState = self.group.get_current_joint_values()
diff = compareStates(curState, self.endState)
print("diff=" + str(diff))
if diff < self.tol or self.timeout + rospy.Duration(
self.watchdog) < rospy.Time.now():
self.count += 1
if self.count > 3:
Logger.loginfo('Target reached :)')
return "done"
else:
self.count = 0
else:
return "done"
def on_enter(self, userdata):
Logger.loginfo('Enter Move Arm')
self.timeout = rospy.Time.now()
if type(userdata.targetPose) is Pose:
Logger.loginfo('target is a pose')
waypoints = [
self.group.get_current_pose().pose,
copy.deepcopy(userdata.targetPose)
]
try:
(plan, fraction) = self.group.compute_cartesian_path(
waypoints, self.movSteps, self.jumpThresh)
except:
Logger.loginfo('Planning failed; could not compute path')
self.result = 'failed'
return
else:
Logger.loginfo('ERROR in ' + str(self.name) +
' : target is not a Pose()')
self.result = 'failed'
Logger.loginfo('target defined')
try:
Logger.loginfo(str(plan))
self.endState = plan.joint_trajectory.points[
len(plan.joint_trajectory.points) - 1].positions
Logger.loginfo(str(self.endState))
except:
Logger.loginfo('Planning failed; path is invalid')
self.result = 'failed'
return
Logger.loginfo('Plan done successfully')
if self.move:
Logger.loginfo('Initiating movement')
self.group.execute(plan, wait=False)
if not self.waitForExecution:
self.result = "done"
else:
Logger.loginfo('The target is reachable')
self.result = "done"
def on_exit(self, userdata):
Logger.loginfo('Stopping movement')
self.group.stop()
def on_pause(self):
Logger.loginfo('Pausing movement')
self.group.stop()
def on_resume(self, userdata):
self.on_enter(userdata)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
cartesian = rospy.get_param('~cartesian', False)
# Connect to the right_arm move group
right_arm = MoveGroupCommander('manipulator')
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame('base_link')
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "straight_forward" configuration stored in the
#3SRDF file
right_arm.set_named_target('top_right')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
# Get the current pose so we can add it as a waypoint
start_pose = right_arm.get_current_pose(end_effector_link).pose
# Initialize the waypoints list
waypoints = []
# Set the first waypoint to be the starting pose
if cartesian:
# Append the pose to the waypoints list
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
# Set the next waypoint back 0.2 meters and right 0.2 meters
wpose.position.x -= 0.2
wpose.position.y -= 0.2
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
right_arm.set_pose_target(wpose)
right_arm.go()
rospy.sleep(1)
# Set the next waypoint to the right 0.15 meters
wpose.position.x += 0.05
wpose.position.y += 0.15
wpose.position.z -= 0.15
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
right_arm.set_pose_target(wpose)
right_arm.go()
rospy.sleep(1)
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(start_pose))
else:
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(1)
if cartesian:
fraction = 0.0
maxtries = 100
attempts = 0
# Set the internal state to the current state
right_arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = right_arm.compute_cartesian_path(
waypoints, # waypoint poses
0.01, # eef_step
0.0, # jump_threshold
True) # avoid_collisions
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
right_arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(maxtries) + " attempts.")
# Move normally back to the 'resting' position
right_arm.set_named_target('bottom_right')
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
right_arm = MoveGroupCommander("right_arm")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
print right_arm.get_current_pose().pose
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 0.570398010274
wpose.orientation.x = -0.279748061111
wpose.orientation.y = -0.719670670452
wpose.orientation.z = -0.280109368412
wpose.position.y = -0.0349149588619
wpose.position.z = 1.69789257029
wpose.position.x = 0.0117939632591
right_arm.set_pose_target(wpose)
plan1 = right_arm.plan()
right_arm.execute(plan1)
print "============ Waiting while RVIZ executes plan1..."
rospy.sleep(5)
waypoints = []
waypoints.append(right_arm.get_current_pose().pose)
print right_arm.get_current_pose().pose
gain = 0.05;
r = 0.05
a = 0.0117939632591 # waypoints[0].position.x
b = 1.64789257 # waypoints[0].position.z
#waypoints[0].position.x = a + r
points = 10
for i in xrange(points):
wpose = geometry_msgs.msg.Pose()
start_pose.orientation.x = 0.5
start_pose.orientation.y = 0.5
start_pose.orientation.z = 0.5
start_pose.orientation.w = 0.5
# start_pose.orientation.w = 0
# start_pose.orientation.x = 0
# start_pose.orientation.y = 1
# start_pose.orientation.z = 0
# start_pose.position.y = 0.0256415233819
# start_pose.position.z = 1.25871460368
# start_pose.position.x = 0.243500142238
right_arm.set_pose_target(start_pose)
plan_start = right_arm.plan()
print "============ Waiting while RVIZ displays plan_start..."
rospy.sleep(5)
right_arm.execute(plan_start)
print "============ Waiting while RVIZ executes plan_start..."
rospy.sleep(5)
waypoints = []
waypoints.append(right_arm.get_current_pose().pose)
gain = 0.1
points = 5
for i in xrange(points):
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = waypoints[i - 1].orientation.w
wpose.orientation.x = waypoints[i - 1].orientation.x
wpose.orientation.y = waypoints[i - 1].orientation.y
wpose.orientation.z = waypoints[i - 1].orientation.z
wpose.position.y = waypoints[i - 1].position.y
right_arm = MoveGroupCommander("right_arm")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
print right_arm.get_current_pose().pose
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 0.468593286845
wpose.orientation.x = -0.0856909255342
wpose.orientation.y = -0.781529322291
wpose.orientation.z = -0.40285148033
wpose.position.y = 0.0925025791124
wpose.position.z = 1.33093409609
wpose.position.x = -0.0834208108295
right_arm.set_pose_target(wpose)
plan1 = right_arm.plan()
right_arm.execute(plan1)
print "============ Waiting while RVIZ executes plan1..."
rospy.sleep(5)
waypoints = []
waypoints.append(right_arm.get_current_pose().pose)
print right_arm.get_current_pose().pose
points = 20
for i in xrange(points):
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = waypoints[i-1].orientation.w
wpose.orientation.x = waypoints[i-1].orientation.x
wpose.orientation.y = waypoints[i-1].orientation.y
wpose.orientation.z = waypoints[i-1].orientation.z
wpose.position.y = waypoints[i-1].position.y
wpose.position.z = waypoints[i-1].position.z + 0.4
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
cartesian = rospy.get_param('~cartesian', True)
# Connect to the arm move group
arm = MoveGroupCommander('arm')
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame('base_link')
# Allow some leeway in position(meters) and orientation (radians)
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Set an initial position for the arm
start_position = [0.0, 0.5, -0.0074579719079, -1.67822729461, -3.1415174069, -1.1, 3.1415174069]
# Set the goal pose of the end effector to the stored pose
arm.set_joint_value_target(start_position)
# Plan and execute a trajectory to the goal configuration
arm.go()
# Get the current pose so we can add it as a waypoint
start_pose = arm.get_current_pose(end_effector_link).pose
# Initialize the waypoints list
waypoints = []
# Set the first waypoint to be the starting pose
if cartesian:
# Append the pose to the waypoints list
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
# Move end effector to the right 0.3 meters
wpose.position.y -= 0.3
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# Move end effector up and back
wpose.position.x -= 0.2
wpose.position.z += 0.3
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(start_pose))
else:
arm.set_pose_target(start_pose)
arm.go()
rospy.sleep(1)
if cartesian:
fraction = 0.0
maxtries = 100
attempts = 0
# Set the internal state to the current state
arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path (
waypoints, # waypoint poses
0.025, # eef_step
0.0, # jump_threshold
True) # avoid_collisions
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
rpys_arm = rect_rpys + circ_rpys
time_list = rect_time + circ_time
print('Limb: %s' % limb_name)
print('Positions (%d): %s' % (len(positions_arm), positions_arm))
print('RPY Pattern (%d): %s' % (len(rpys_arm), rpys_arm))
print('Time List (%d): %s' % (len(time_list), time_list))
# plan waypoints
rospy.loginfo("Planing Waypoints")
plan, flaction = setTargetPoseSequenceMoveIt(group, positions_arm,
rpys_arm, time_list)
# wait for playing the plan on MoveIt
rospy.loginfo("Waiting while the plan is visualized on RViz")
rospy.sleep(20)
# execute the plan
rospy.loginfo("Execute!!")
group.execute(plan)
rospy.loginfo("Executiion Done")
rospy.sleep(5)
# move to the initial pose
rospy.loginfo("Move to Initial Pose")
pose_target = copy.deepcopy(rarm_initial_pose)
rospy.loginfo("set target to {}".format(pose_target))
group.set_pose_target(pose_target)
plan = group.plan()
ret = group.go()
print traj_approach
### Set next (virtual) start state
traj_approach_endpoint = traj_approach.joint_trajectory.points[-1]
start_state = RobotState()
#start_state.header = traj_approach.header
#start_state.header.stamp = rospy.Time.now()
start_state.joint_state.name = traj_approach.joint_trajectory.joint_names
start_state.joint_state.position = traj_approach_endpoint.positions
start_state.is_diff = True
### Plan Lift
goal_pose.position.z += 0.1
mgc.set_start_state(start_state)
(traj_lift, frac_lift) = mgc.compute_cartesian_path([goal_pose], 0.01, 4,
True)
print frac_lift
print traj_lift
if not (frac_approach == 1.0 and frac_lift == 1.0):
rospy.logerr("Unable to plan whole grasping trajectory")
else:
sss.move("arm_right", "pre_grasp")
# sss.move("gripper_right", "open")
mgc.execute(traj_approach)
# sss.move("gripper_right", "close")
mgc.execute(traj_lift)
sss.move("arm_right", "pre_grasp")
print "Done"
right_arm = MoveGroupCommander("right_arm")
start_pose = geometry_msgs.msg.Pose()
start_pose.position.x = -0.0206330384032
start_pose.position.y = 0.077582282778
start_pose.position.z = 1.39283677496
start_pose.orientation.x = 0.5
start_pose.orientation.y = 0.5
start_pose.orientation.z = 0.5
start_pose.orientation.w = 0.5
right_arm.set_pose_target(start_pose)
plan_start = right_arm.plan()
print "Plan start"
rospy.sleep(5)
right_arm.execute(plan_start)
print "Execute start"
rospy.sleep(5)
end_pose = geometry_msgs.msg.Pose()
end_pose.position.x = -0.0434279649929
end_pose.position.y = -0.0562017053887
end_pose.position.z = 1.48763433664
end_pose.orientation.x = 0.5
end_pose.orientation.y = 0.5
end_pose.orientation.z = 0.5
end_pose.orientation.w = 0.5
right_arm.set_pose_target(end_pose)
plan_end = right_arm.plan()
print "Plan end"
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
self.scene=PlanningSceneInterface()
self.scene_pub=rospy.Publisher('planning_scene',PlanningScene)
#self.gripperCtrl=rospy.ServiceProxy("/two_finger/gripper/gotoPositionUntilTouch",SetPosition)
#self.m2j=rospy.Publisher("/two_finger/motoman_control/move_to_joint",JointAnglesDuration,queue_size=1,latch=True)
self.colors=dict()
rospy.sleep(1)
arm=MoveGroupCommander('arm')
cartesian=rospy.get_param('~cartesian',True)
#gripper=MoveGroupCommander('gripper')
self.arm_end_effector_link=arm.get_end_effector_link()
#gripper_end_effector_link=gripper.get_end_effector_link()
arm.set_goal_position_tolerance(0.005)
arm.set_goal_orientation_tolerance(0.025)
arm.allow_replanning(True)
#gripper.set_goal_position_tolerance(0.01)
#gripper.set_goal_orientation_tolerance(0.05)
#gripper.allow_replanning(True)
self.reference_frame='base_link'
arm.set_pose_reference_frame(self.reference_frame)
#gripper.set_pose_reference_frame(reference_frame)
arm.set_planning_time(5)
#gripper.set_planning_time(5)
#scene planning
self.table_id='table'
self.box2_id='box2'
self.target_id='target_object'
#scene.remove_world_object(box1_id)
self.scene.remove_world_object(self.box2_id)
self.scene.remove_world_object(self.table_id)
self.scene.remove_world_object(self.target_id)
self.table_ground=0.5
self.table_size=[0.5,1,0.01]
self.setupSence()
target_size=[0.05,0.05,0.1]
#target
target_pose=PoseStamped()
target_pose.header.frame_id=self.reference_frame
target_pose.pose.position.x=0.6
target_pose.pose.position.y=0.05
target_pose.pose.position.z=self.table_ground+self.table_size[2]+target_size[2]/2.0
target_pose.pose.orientation.x=0
target_pose.pose.orientation.y=0
target_pose.pose.orientation.z=0
target_pose.pose.orientation.w=1
self.scene.add_box(self.target_id,target_pose,target_size)
#grasp
g_p=PoseStamped()
g_p.header.frame_id=self.arm_end_effector_link
g_p.pose.position.x=0.00
g_p.pose.position.y=-0.00
g_p.pose.position.z=0.025
g_p.pose.orientation.w=0.707
g_p.pose.orientation.x=0
g_p.pose.orientation.y=-0.707
g_p.pose.orientation.z=0
#set color
self.setColor(self.table_id,0.8,0,0,1.0)
#self.setColor(self.box2_id,0.8,0.4,0,1.0)
self.setColor('r_tool',0.8,0,0)
self.setColor('l_tool',0.8,0,0)
self.setColor(self.target_id,0,1,0)
self.sendColors()
#motion planning
arm.set_named_target("slight")
arm.go()
rospy.sleep(2)
arm.set_named_target("initial_arm")
arm.go()
rospy.sleep(2)
start_pose=arm.get_current_pose(self.arm_end_effector_link).pose
rospy.sleep(2)
grasp_pose=target_pose
grasp_pose.pose.position.x-=0.15
#grasp_pose.pose.position.z=
grasp_pose.pose.orientation.x=0
grasp_pose.pose.orientation.y=0.707
grasp_pose.pose.orientation.z=0
grasp_pose.pose.orientation.w=0.707
waypoints_1=[]
waypoints_2=[]
if cartesian:
waypoints_1.append(start_pose)
waypoints_1.append(deepcopy(grasp_pose.pose))
#waypoints_1.append(deepcopy(grasp_pose.pose))
fraction=0.0
#maxtries=300
attempts=0
#arm.set_start_state__1to_current_state()
#plan the cartesian path connecting waypoints
#while fraction<1.0 and attempts<maxtries:
while fraction!=1:
(plan_1,fraction)=arm.compute_cartesian_path(waypoints_1,0.01,0.0,True)
attempts+=1
if (attempts %300==0 and fraction!=1.0):
rospy.loginfo("path planning failed with " + str(attempts)+" attempts")
if KeyboardInterrupt:
break
rospy.loginfo("path compute successfully with "+str(attempts)+" sttempts.")
rospy.loginfo("Arm moving.")
arm.execute(plan_1)
rospy.sleep(6)
#remove and shut down
#self.scene.remove_attached_object(self.arm_end_effector_link,'l_tool')
#self.scene.remove_attached_object(self.arm_end_effector_link,'r_tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
while rospy.get_time() == 0.0: pass
### Create a handle for the Planning Scene Interface
psi = PlanningSceneInterface()
rospy.sleep(1.0)
### Create a handle for the Move Group Commander
mgc = MoveGroupCommander("arm")
rospy.sleep(1.0)
### Add virtual obstacle
pose = gen_pose(pos=[-0.2, -0.1, 1.2])
psi.add_box("box", pose, size=(0.15, 0.15, 0.6))
rospy.sleep(1.0)
### Move to stored joint position
mgc.set_named_target("left")
mgc.go()
### Move to Cartesian position
goal_pose = gen_pose(pos=[0.123, -0.417, 1.361], euler=[3.1415, 0.0, 1.5707])
mgc.go(goal_pose.pose)
### Move Cartesian linear
goal_pose.pose.position.z -= 0.1
(traj,frac) = mgc.compute_cartesian_path([goal_pose.pose], 0.01, 4, True)
mgc.execute(traj)
print "Done"
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('eight_trajectory_planning', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.001)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(1)
arm.set_max_velocity_scaling_factor(1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂先回到初始化位置
arm.set_named_target('start')
arm.go()
rospy.sleep(1)
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.38665
target_pose.pose.position.y = 0
target_pose.pose.position.z = 0.37226
target_pose.pose.orientation.x = 1
target_pose.pose.orientation.y = 0.00007
target_pose.pose.orientation.z = -0.0024684
target_pose.pose.orientation.w = 0.00003
# 设置机械臂终端运动的目标位姿
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
# 初始化路点列表
waypoints = []
# 将圆弧上的路径点加入列表
waypoints.append(target_pose.pose)
centerA = target_pose.pose.position.y
centerB = target_pose.pose.position.z
radius = 0.04
for th in numpy.arange(0, 6.284, 0.005):
target_pose.pose.position.y = centerA + radius * math.cos(th -
1.571)
target_pose.pose.position.z = centerB + radius * math.sin(
th - 1.571) + 0.04
wpose = deepcopy(target_pose.pose)
waypoints.append(deepcopy(wpose))
for th in numpy.arange(0, 6.284, 0.005):
target_pose.pose.position.y = centerA + radius * math.cos(-th +
1.571)
target_pose.pose.position.z = centerB + radius * math.sin(
-th + 1.571) - 0.04
wpose = deepcopy(target_pose.pose)
waypoints.append(deepcopy(wpose))
#print('%f, %f' % (Y, Z))
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点，完成圆弧轨迹
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses，路点列表
0.01, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(maxtries) + " attempts.")
rospy.sleep(1)
rospy.loginfo("位姿控制---->八字型轨迹规划 成功!")
# 控制机械臂先回到初始化位置
arm.set_named_target('start')
arm.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
right_arm = MoveGroupCommander("right_arm")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
print right_arm.get_current_pose().pose
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 0.0176318609849
wpose.orientation.x = 0.392651866792
wpose.orientation.y = 0.918465607415
wpose.orientation.z = 0.0439835989663
wpose.position.y = 0.227115629827
wpose.position.z = 1.32344046934
wpose.position.x = -0.358178766726
right_arm.set_pose_target(wpose)
plan1 = right_arm.plan()
right_arm.execute(plan1)
print "============ Waiting while RVIZ executes plan1..."
rospy.sleep(5)
waypoints = []
waypoints.append(right_arm.get_current_pose().pose)
print right_arm.get_current_pose().pose
points = 20
for i in xrange(points):
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = waypoints[i-1].orientation.w + 0.5285
wpose.orientation.x = waypoints[i-1].orientation.x - 0.6736
wpose.orientation.y = waypoints[i-1].orientation.y - 1.638
wpose.orientation.z = waypoints[i-1].orientation.z - 0.308
wpose.position.y = waypoints[i-1].position.y - 0.1
wpose.position.z = waypoints[i-1].position.z
class WarehousePlanner(object):
def __init__(self):
rospy.init_node('moveit_warehouse_planner', anonymous=True)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
rospy.sleep(2)
group_id = rospy.get_param("~arm_group_name")
self.eef_step = rospy.get_param("~eef_step", 0.01)
self.jump_threshold = rospy.get_param("~jump_threshold", 1000)
self.group = MoveGroupCommander(group_id)
# self._add_ground()
self._robot_name = self.robot._r.get_robot_name()
self._robot_link = self.group.get_end_effector_link()
self._robot_frame = self.group.get_pose_reference_frame()
self._min_wp_fraction = rospy.get_param("~min_waypoint_fraction", 0.9)
rospy.sleep(4)
rospy.loginfo("Waiting for warehouse services...")
rospy.wait_for_service('/list_robot_states')
rospy.wait_for_service('/get_robot_state')
rospy.wait_for_service('/has_robot_state')
rospy.wait_for_service('/compute_fk')
self._list_states = rospy.ServiceProxy('/list_robot_states',
ListStates)
self._get_state = rospy.ServiceProxy('/get_robot_state', GetState)
self._has_state = rospy.ServiceProxy('/has_robot_state', HasState)
self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK)
rospy.loginfo("Service proxies connected")
self._tr_frm_list_srv = rospy.Service('plan_trajectory_from_list',
PlanTrajectoryFromList,
self._plan_from_list_cb)
self._tr_frm_prfx_srv = rospy.Service('plan_trajectory_from_prefix',
PlanTrajectoryFromPrefix,
self._plan_from_prefix_cb)
self._execute_tr_srv = rospy.Service('execute_planned_trajectory',
ExecutePlannedTrajectory,
self._execute_plan_cb)
self.__plan = None
def get_waypoint_names_by_prefix(self, prefix):
regex = "^" + str(prefix) + ".*"
waypoint_names = self._list_states(regex, self._robot_name).states
return waypoint_names
def get_pose_from_state(self, state):
header = Header()
fk_link_names = [self._robot_link]
header.frame_id = self._robot_frame
response = self._forward_k(header, fk_link_names, state)
return response.pose_stamped[0].pose
def get_cartesian_waypoints(self, waypoint_names):
waypoints = []
waypoints.append(self.group.get_current_pose().pose)
for name in waypoint_names:
if self._has_state(name, self._robot_name).exists:
robot_state = self._get_state(name, "").state
waypoints.append(self.get_pose_from_state(robot_state))
else:
rospy.logerr("Waypoint %s doesn't exist for robot %s.", name,
self._robot_name)
return waypoints
def _add_ground(self):
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = 0
p.pose.position.y = 0
# offset such that the box is below the dome
p.pose.position.z = -0.11
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
self.scene.add_box("ground", p, (3, 3, 0.01))
rospy.sleep(1)
def plan_from_filter(self, prefix):
waypoint_names = self.get_waypoint_names_by_prefix(prefix)
waypoint_names.sort()
if 0 == len(waypoint_names):
rospy.logerr(
"No waypoints found for robot %s with prefix %s. " +
"Can't make trajectory :(", self._robot_name, str(prefix))
return False
rospy.loginfo(
"Creating trajectory with %d waypoints selected by " +
"prefix %s.", len(waypoint_names), str(prefix))
return self.plan_from_list(waypoint_names)
def plan_from_list(self, waypoint_names):
self.group.clear_pose_targets()
waypoints = self.get_cartesian_waypoints(waypoint_names)
if len(waypoints) != len(waypoint_names) + 1:
# +1 because current position is used as first waypiont.
rospy.logerr("Not all waypoints existed, not executing.")
return False
(plan,
fraction) = self.group.compute_cartesian_path(waypoints,
self.eef_step,
self.jump_threshold)
if fraction < self._min_wp_fraction:
rospy.logerr(
"Only managed to generate trajectory through" +
"%f of waypoints. Not executing", fraction)
return False
self.__plan = plan
return True
def _plan_from_list_cb(self, request):
# check all exist
self.__plan = None
rospy.loginfo("Creating trajectory from points given: %s",
",".join(request.waypoint_names))
return self.plan_from_list(request.waypoint_names)
def _plan_from_prefix_cb(self, request):
self.__plan = None
rospy.loginfo("Creating trajectory from points filtered by prefix %s",
request.prefix)
return self.plan_from_filter(request.prefix)
def _execute_plan_cb(self, request):
if self.__plan is None:
rospy.logerr("No plan stored!")
return False
rospy.loginfo("Executing stored plan")
response = self.group.execute(self.__plan)
self.__plan = None
return response
