def plan(self):
dt = DisplayTrajectory()
dt.trajectory_start = self.robot.get_current_state()
plan = self.group.plan()
dt.trajectory = [plan]
self.dt_pub.publish(dt)
return plan
def visualize_movement(start, path):
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path', DisplayTrajectory,
queue_size=100) # for visualizing the robot movement;
sleep(0.5)
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = convertStateToRobotState(start)
trajectory = RobotTrajectory()
points = []
joint_trajectory = JointTrajectory()
joint_trajectory.joint_names = get_joint_names('right')
for state, _ in path:
point = JointTrajectoryPoint()
point.positions = convertStateToRobotState(state).joint_state.position
points.append(point)
joint_trajectory.points = points
trajectory.joint_trajectory = joint_trajectory
# print("The joint trajectory is: %s" % trajectory)
display_trajectory.trajectory.append(trajectory)
display_trajectory_publisher.publish(display_trajectory)
def displayTrajFromPlan(self, plan, joint_names, initial_state):
"""Given a plan (GetDMPPlanResponse), joint_names list and an initial_state as RobotState
Create a displayTraj message"""
dt = DisplayTrajectory()
dt.trajectory.append( self.robotTrajectoryFromPlan(plan, joint_names) )
dt.trajectory_start = self.robotStateFromJoints(joint_names, initial_state)
dt.model_id = "reem"
return dt
def displayTrajFromRobotTraj(self, robot_traj):
"""Given a robot trajectory return a displaytrajectory with it"""
dt = DisplayTrajectory()
dt.trajectory.append(robot_traj)
dt.trajectory_start.joint_state.name = robot_traj.joint_trajectory.joint_names
dt.trajectory_start.joint_state.position = robot_traj.joint_trajectory.points[0].positions
dt.model_id = "reem"
return dt
def visualize_plan(self, plan): # Untested
"""
Visualize the plan in RViz
"""
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = plan.points[0]
display_trajectory.trajectory.extend(plan.points)
self.display_planned_path_publisher.publish(display_trajectory)
def get_display_trajectory(self, *joint_trajectories):
display_trajectory = DisplayTrajectory()
display_trajectory.model_id = 'pr2'
for joint_trajectory in joint_trajectories:
robot_trajectory = RobotTrajectory()
robot_trajectory.joint_trajectory = joint_trajectory
# robot_trajectory.multi_dof_joint_trajectory = ...
display_trajectory.trajectory.append(robot_trajectory)
display_trajectory.trajectory_start = self.get_robot_state()
return display_trajectory
def publish_trajectory(self, plan):
# type: (RobotTrajectory) -> None
"""
Publish trajectory so that we can visualize in Rviz
:param plan:
:return: None
"""
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = self.get_robot_state()
display_trajectory.trajectory.append(plan)
self.display_trajectory_publisher.publish(display_trajectory)
def display_planned_path(self, path):
"""Sends the trajectory to apropriate topic to be displayed.
Arguments:
path : {RobotTrajectory message} Trajectory to be displayed
"""
self.pub = rospy.Publisher(TOPIC_DISPLAY_PLANNED_PATH,
DisplayTrajectory)
dsp = DisplayTrajectory()
dsp.trajectory = [path]
self.pub.publish(dsp)
def _visualize_plan(self, data_store):
# Pull out the plan from the data
full_plan = [x.plan for x in data_store[self.PLAN_OBJ_KEY]]
plan = self._merge_plans(full_plan)
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = self.arm_planner.robot.get_current_state(
)
display_trajectory.trajectory.append(plan)
self.display_trajectory_publisher.publish(display_trajectory)
return plan
def visualize_path(self, path):
'''
path will be a list of grid cells
'''
current_state = self.robot.get_current_state()
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = current_state
path_config = [self._grid_to_continuous(cell) for cell in path]
display_trajectory.trajectory.append(self.get_robot_trajectory_msg(
path_config))
self.display_trajectory_publisher.publish(display_trajectory)
def moveArmCartesian(self, config):
step = 0.01
attempts = 1
waypoints = []
pose_goal = self.group.get_current_pose().pose
waypoints.append(pose_goal) # current pose
pose_goal.orientation = geometry_msgs.msg.Quaternion(
*tf_conversions.transformations.quaternion_from_euler(
0., -math.pi / 2, 0.))
pose_goal.position.x = config.x
pose_goal.position.y = config.y
pose_goal.position.z = config.z
waypoints.append(copy.deepcopy(pose_goal)) # goal pose
print "- Planning Move..."
# create cartesian plan based on current and goal pose
# compute_cartesian_path: Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints.
# Configurations are computed for every eef_step meters;
# The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resulting path.
# The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory.
# avoid_collision = True
(plan,
fraction) = self.group.compute_cartesian_path(waypoints, step, 0.0)
rospy.sleep(0.5)
print "%% success:", fraction
while (fraction < 0.8) and (
attempts <= 4
): # force a somewhat successful plan by adding more intermediary points
attempts += 1
step = step / 2
print "%% success:", fraction
(plan, fraction) = self.group.compute_cartesian_path(
waypoints, step, 0.0)
rospy.sleep(0.5)
# check if the move was successful
if fraction > 0.8:
print "Success.-"
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = self.robot.get_current_state(
)
display_trajectory.trajectory.append(plan)
self.display_trajectory_publisher.publish(display_trajectory)
rospy.sleep(1)
print "- Executing..."
self.group.execute(plan, wait=True)
rospy.sleep(1)
self.reachable = True
else:
print "Fail.-"
self.reachable = False
def execute(self, traj, wait=True, display=True):
""" Execute the trajectory on the robot arm """
if traj:
rospy.loginfo("Executing trajectory")
if display:
# Display the planned trajectory
display_traj = DisplayTrajectory()
display_traj.trajectory_start = self.robot.get_current_state()
display_traj.trajectory.append(traj)
self.display_trajectory_pub.publish(display_traj)
# Execute the trajectory
self.arm_group.execute(traj, wait=wait)
def callback(self, anymsg, topic_name):
if self.pause_button.isChecked():
return
# In case of control_msgs/FollowJointTrajectoryActionGoal
# set trajectory_msgs/JointTrajectory to 'msg'
# Convert AnyMsg to trajectory_msgs/JointTrajectory
msg_class = self.topic_name_class_map[topic_name]
if msg_class == JointTrajectory:
msg = JointTrajectory().deserialize(anymsg._buff)
elif msg_class == FollowJointTrajectoryActionGoal:
msg = FollowJointTrajectoryActionGoal().deserialize(
anymsg._buff).goal.trajectory
elif msg_class == DisplayTrajectory:
if DisplayTrajectory().deserialize(
anymsg._buff).trajectory.__len__() > 0:
msg = DisplayTrajectory().deserialize(
anymsg._buff).trajectory.pop().joint_trajectory
else:
rospy.logwarn(
"Received planned trajectory has no waypoints in it. Nothing to plot!"
)
return
else:
rospy.logerr('Wrong message type %s' % msg_class)
return
self.time = np.array([0.0] * len(msg.points))
(self.dis, self.vel, self.acc, self.eff) = ({}, {}, {}, {})
for joint_name in msg.joint_names:
self.dis[joint_name] = np.array([0.0] * len(msg.points))
self.vel[joint_name] = np.array([0.0] * len(msg.points))
self.acc[joint_name] = np.array([0.0] * len(msg.points))
self.eff[joint_name] = np.array([0.0] * len(msg.points))
for i in range(len(msg.points)):
point = msg.points[i]
self.time[i] = point.time_from_start.to_sec()
for j in range(len(msg.joint_names)):
joint_name = msg.joint_names[j]
if point.positions:
self.dis[joint_name][i] = point.positions[j]
if point.velocities:
self.vel[joint_name][i] = point.velocities[j]
if point.accelerations:
self.acc[joint_name][i] = point.accelerations[j]
if point.effort:
self.eff[joint_name][i] = point.effort[j]
if self.joint_names != msg.joint_names:
self.joint_names = msg.joint_names
self.refresh_tree()
self.joint_names = msg.joint_names
self.plot_graph()
def viz_joint_traj(self, j_traj, base_link='base_link'):
robot_tr = RobotTrajectory()
j_traj.header.frame_id = base_link
robot_tr.joint_trajectory = j_traj
disp_tr = DisplayTrajectory()
disp_tr.trajectory.append(robot_tr)
disp_tr.model_id = self.robot_name
i = 0
while (not rospy.is_shutdown() and i < 10):
i += 1
self.pub_tr.publish(disp_tr)
self.rate.sleep()
def plan(samplerIndex, start, goal, ss, space, display_trajectory_publisher):
si = ss.getSpaceInformation()
if samplerIndex == 1:
# use obstacle-based sampling
space.setStateSamplerAllocator(
ob.StateSamplerAllocator(allocSelfCollisionFreeStateSampler))
ss.setStartAndGoalStates(start, goal)
# create a planner for the defined space
planner = og.FMT(si) # change this to FMT;
if samplerIndex == 1:
planner.setVAEFMT(
1) # This flag is for turning on sampling with the VAE generator;
# set parameter;
planner.setExtendedFMT(
False) # do not extend if the planner does not terminate;
planner.setNumSamples(100)
planner.setNearestK(False)
planner.setCacheCC(True)
planner.setHeuristics(True)
# planner.setNearestK(1) # Disable K nearest neighbor implementation;
ss.setPlanner(planner)
start_time = time.time()
solved = ss.solve(40.0)
elapsed_time = time.time() - start_time
if solved:
print("Found solution after %s seconds:" % elapsed_time)
# print the path to screen
path = ss.getSolutionPath()
# ("The solution is: %s" % path)
# Visualization
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = convertStateToRobotState(start)
trajectory = convertPlanToTrajectory(path)
display_trajectory.trajectory.append(trajectory)
display_trajectory_publisher.publish(display_trajectory)
print("Visualizing trajectory...")
sleep(0.5)
else:
print("No solution found after %s seconds: " % elapsed_time)
return elapsed_time, float((int(solved)))
def publish_traj(self):
display_trajectory = DisplayTrajectory()
if self.trajectory[0]:
display_trajectory.trajectory.append(self.trajectory[1])
self._display_trajectory_publisher.publish(display_trajectory)
else:
rospy.logwarn("No trajectory to view")
def handle_show_request(self, req):
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = self.robot.get_current_state()
#build moveit msg for display
joint_multi_traj_msg = MultiDOFJointTrajectory()
robot_traj_msg = RobotTrajectory()
robot_traj_msg.joint_trajectory = req.JOINT_TRAJECTORY
robot_traj_msg.multi_dof_joint_trajectory = joint_multi_traj_msg
display_trajectory.trajectory.append(robot_traj_msg)
rospy.loginfo("PlanningService::handle_show_request() -- showing trajectory %s", req.JOINT_TRAJECTORY)
self.display_trajectory_publisher.publish(display_trajectory);
return True
def display(self, trajectory):
"""
Display a joint-space trajectory or a robot state in RVIz loaded with the Moveit plugin
:param trajectory: a RobotTrajectory, JointTrajectory, RobotState or JointState message
"""
# Publish the DisplayTrajectory (only for trajectory preview in RViz)
# includes a convert of float durations in rospy.Duration()
def js_to_rt(js):
rt = RobotTrajectory()
rt.joint_trajectory.joint_names = js.name
rt.joint_trajectory.points.append(
JointTrajectoryPoint(positions=js.position))
return rt
dt = DisplayTrajectory()
if isinstance(trajectory, RobotTrajectory):
dt.trajectory.append(trajectory)
elif isinstance(trajectory, JointTrajectory):
rt = RobotTrajectory()
rt.joint_trajectory = trajectory
dt.trajectory.append(rt)
elif isinstance(trajectory, RobotState):
dt.trajectory.append(js_to_rt(trajectory.joint_state))
elif isinstance(trajectory, JointState):
dt.trajectory.append(js_to_rt(trajectory))
else:
raise NotImplementedError(
"ArmCommander.display() expected type RobotTrajectory, JointTrajectory, RobotState or JointState, got {}"
.format(str(type(trajectory))))
self._display_traj.publish(dt)
def plan_trajectory_in_cspace(self, goal, visualization=True):
## Then, we will get the current set of joint values for the group
self.group.set_joint_value_target(goal)
plan = self.group.plan()
if visualization:
print "============ Visualizing trajectory_plan"
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = self.robot.get_current_state(
)
display_trajectory.trajectory.append(plan)
self.display_trajectory_publisher.publish(display_trajectory)
print "============ Waiting while plan is visualized (again)..."
rospy.sleep(5)
return plan
def execute(self):
rospy.loginfo("Start Task")
# Get latest task plan
plan = self.task_q[0]
for points in plan.trajectory.joint_trajectory.points:
tfs = points.time_from_start.to_sec()
tfs /= self.exe_speed_rate
points.time_from_start = rospy.Duration(tfs)
scaled_vel = []
scaled_acc = []
for vel in points.velocities:
scaled_vel.append(vel * self.exe_speed_rate)
points.velocities = tuple(scaled_vel)
for acc in points.accelerations:
scaled_acc.append(acc * self.exe_speed_rate *
self.exe_speed_rate)
points.accelerations = tuple(scaled_acc)
# Display the Trajectory
start_state = JointState()
start_state.header = Header()
start_state.header.stamp = rospy.Time.now()
start_state.name = plan.trajectory.joint_trajectory.joint_names[:]
start_state.position = plan.trajectory.joint_trajectory.points[
-1].positions[:]
moveit_start_state = RobotState()
moveit_start_state.joint_state = start_state
pub_display_msg = DisplayTrajectory()
pub_display_msg.model_id = "vs087"
pub_display_msg.trajectory.append(plan.trajectory)
pub_display_msg.trajectory_start = moveit_start_state
self.display_hp_pub.publish(pub_display_msg)
# Send Action and Wait result
goal = ExecutePlanAction().action_goal.goal
goal.planning_time = plan.planning_time
goal.start_state = plan.start_state
# goal.start_state.joint_state.header.stamp = rospy.Time.now()
goal.trajectory = plan.trajectory
# rospy.logwarn(goal)
self.client.send_goal(goal)
self.client.wait_for_result()
# Update the task queue
self.task_q.pop(0)
rospy.loginfo("End Task")
def display(self, trajectory):
if isinstance(trajectory, RobotTrajectory):
trajectory = trajectory.joint_trajectory
if not isinstance(trajectory, JointTrajectory):
rospy.logerr("robot.display() only accepts joint trajectories")
return
dt = DisplayTrajectory()
rt = RobotTrajectory(joint_trajectory=trajectory)
dt.trajectory.append(rt)
self._display_pub.publish(dt)
def visualize_trajectory(self, blocking=True):
if not self.display:
return
if not self.single_arm:
self.adjust_traj_length()
if self.rviz_pub.get_num_connections() < 1:
rospy.logerr("Rviz topic not subscribed")
return
msg = DisplayTrajectory()
msg.trajectory_start = self.robot_model.get_current_state()
traj = RobotTrajectory()
goal = JointTrajectory()
goal.joint_names = self.left.JOINT_NAMES[:]
if not self.single_arm:
goal.joint_names += self.right.JOINT_NAMES[:]
# The sim is very slow if the number of points is too large
steps = 1 if len(self.left._goal.trajectory.points) < 100 else 10
for idx in range(0, len(self.left._goal.trajectory.points), steps):
comb_point = JointTrajectoryPoint()
lpt = self.left._goal.trajectory.points[idx]
comb_point.positions = lpt.positions[:]
if not self.single_arm:
comb_point.positions += self.right._goal.trajectory.points[idx].positions[:]
comb_point.time_from_start = lpt.time_from_start
goal.points.append(comb_point)
traj.joint_trajectory = goal
msg.trajectory.append(traj)
duration = goal.points[-1].time_from_start.to_sec()
self.rviz_pub.publish(msg)
if blocking:
rospy.loginfo("Waiting for trajectory animation {} seconds".format(duration))
rospy.sleep(duration)
def send_traj(self, u_arr):
# Formulate joint trajectory message:
jt_msg = JointTrajectory()
jt_msg.joint_names = self.joint_names
for i in range(len(u_arr)):
u = u_arr[i]
jt_pt = JointTrajectoryPoint()
jt_pt.positions = u
jt_msg.points.append(jt_pt)
robot_tr = RobotTrajectory()
robot_tr.joint_trajectory = jt_msg
disp_tr = DisplayTrajectory()
disp_tr.trajectory.append(robot_tr)
disp_tr.model_id = self.robot_name
self.pub_tr.publish(disp_tr)
i = 0
while (not rospy.is_shutdown() and i < 10):
i += 1
self.rate.sleep()
def visualize_path(robot_trajectory, position, planned_path_pub, goal_pos_pub):
disp_traj = DisplayTrajectory()
disp_traj.trajectory.append(robot_trajectory)
disp_traj.trajectory_start = RobotState()
planned_path_pub.publish(disp_traj)
marker = Marker()
marker.header.frame_id = "base"
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 1.0
marker.pose.position.x = position[0]
marker.pose.position.y = position[1]
marker.pose.position.z = position[2]
print 'Goal position {0}'.format(position)
goal_pos_pub.publish(marker)
def create_display_traj(self, joint_traj):
self.display_traj = DisplayTrajectory()
self.display_traj.model_id = 'lbr4'
robot_traj = RobotTrajectory()
robot_traj.joint_trajectory = joint_traj
self.display_traj.trajectory.append(robot_traj)
self.display_traj.trajectory_start.\
joint_state.header.frame_id = '/world'
self.display_traj.trajectory_start.joint_state.name = \
self.kdl_kin.get_joint_names()
self.display_traj.trajectory_start.joint_state.position = \
joint_traj.points[0].positions
self.display_traj.trajectory_start.joint_state.velocity = \
joint_traj.points[0].velocities
def execute(self):
# Get latest task plan
plan = self.task_q[0].trajectory
for points in plan.joint_trajectory.points:
tfs = points.time_from_start.to_sec()
tfs /= self.exe_speed_rate
points.time_from_start = rospy.Duration(tfs)
self.grasp_[0] = self.task_q[0].grasp
# Display the Trajectory
start_state = JointState()
start_state.header = Header()
start_state.header.stamp = rospy.Time.now()
start_state.name = plan.joint_trajectory.joint_names[:]
start_state.position = plan.joint_trajectory.points[-1].positions[:]
moveit_start_state = RobotState()
moveit_start_state.joint_state = start_state
pub_display_msg = DisplayTrajectory()
pub_display_msg.model_id = "sia5"
pub_display_msg.trajectory.append(plan)
pub_display_msg.trajectory_start = moveit_start_state
self.display_hp_pub.publish(pub_display_msg)
# Send Action and Wait result
goal = FollowJointTrajectoryGoal(trajectory=plan.joint_trajectory)
rospy.loginfo("Start Task")
self.client.send_goal(goal)
self.client.wait_for_result()
# Grasping
if self.grasp_[0] != self.grasp_[1]:
self.executeGrasp(self.grasp_[0])
self.grasp_[1] = self.grasp_[0]
# Update the task queue
self.task_q.pop(0)
rospy.loginfo("End Task")
def moveArm(self, config):
print "- Planning Move..."
pose_goal = self.group.get_current_pose().pose
pose_goal.orientation = geometry_msgs.msg.Quaternion(
*tf_conversions.transformations.quaternion_from_euler(
0., -math.pi / 2, 0.))
pose_goal.position.x = config.x
pose_goal.position.y = config.y
pose_goal.position.z = config.z
self.group.set_pose_target(pose_goal)
plan = self.group.plan()
rospy.sleep(0.5)
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = self.robot.get_current_state()
display_trajectory.trajectory.append(plan)
self.display_trajectory_publisher.publish(display_trajectory)
rospy.sleep(1)
print "- Executing..."
self.group.go(wait=True)
rospy.sleep(1)
def display_trajectory(self, plan):
print(plan)
traj = plan.trajectory
joints = traj.joint_names
print(traj)
init_joints = JointState()
init_joints.name = joints
init_joints.position = traj.points[0].positions
init_joints.velocity = traj.points[0].velocities
init_joints.effort = traj.points[0].effort
joint_traj = RobotTrajectory()
joint_traj.joint_trajectory = plan.trajectory
init_state = RobotState()
init_state.joint_state = init_joints
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = init_state
display_trajectory.trajectory.append(joint_traj)
# Publish
self.display_trajectory_publisher.publish(display_trajectory)
rospy.sleep(3)
def single_shot(path, obstacles):
exp = DiffCoplusBaxterExperiments()
print('Adding box')
rospy.sleep(2)
box_names = []
## Commented out because assuming obstacles already exist in scene
# for i, obs in enumerate(obstacles):
# box_name = 'box_{}'.format(i)
# box_names.append(box_name)
# box_pose = geometry_msgs.msg.PoseStamped()
# box_pose.header.frame_id = exp.planning_frame
# # box_pose.pose.orientation.w = 1.0
# box_pose.pose.position.x = obs[1][0]
# box_pose.pose.position.y = obs[1][1]
# box_pose.pose.position.z = obs[1][2]
# exp.scene.add_box(box_name, box_pose, size=obs[2])
# wait_for_state_update(exp.scene, box_name, box_is_known=True)
pub = exp.display_trajectory_publisher
joint_traj = JointTrajectory()
for q in path:
traj_point = JointTrajectoryPoint()
traj_point.positions = q.numpy().tolist()
joint_traj.points.append(traj_point)
joint_traj.joint_names = [
'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1',
'left_w2'
]
robot_traj = RobotTrajectory()
robot_traj.joint_trajectory = joint_traj
disp_traj = DisplayTrajectory()
disp_traj.trajectory.append(robot_traj)
disp_traj.trajectory_start.joint_state.position = path[0].numpy()
disp_traj.trajectory_start.joint_state.name = joint_traj.joint_names
pub.publish(disp_traj)
return box_names, exp
def poseCallback(Pose_Array):
print type(Pose_Array)
posearray=Pose_Array.posearray
print (len(posearray))
i=80
for pose in posearray[80:]:
i=i+1
data=pose.pose
#print (type(data))
print (data)
try:
dataCopy1 = copy.deepcopy(data)
(r, p, y) = tf.transformations.euler_from_quaternion([data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w])
tMatrix = tf.transformations.euler_matrix(r, p, y)
x_offset=-0.10
data.pose.position.x=data.pose.position.x+(tMatrix[0,0]*x_offset)
data.pose.position.y=data.pose.position.y + (tMatrix[1,0]*x_offset)
data.pose.position.z=data.pose.position.z + (tMatrix[2,0]*x_offset)
newRotMatrix = tf.transformations.euler_matrix(0, 1.57, 0)
transformedMatrix = numpy.dot(tMatrix, newRotMatrix)
dataCopy1.pose.position.x=dataCopy1.pose.position.x - (transformedMatrix[0,2]*x_offset)
dataCopy1.pose.position.y=dataCopy1.pose.position.y - (transformedMatrix[1,2]*x_offset)
dataCopy1.pose.position.z=dataCopy1.pose.position.z - (transformedMatrix[2,2]*x_offset)
dis = (dataCopy1.pose.position.x - data.pose.position.x) * (dataCopy1.pose.position.x - data.pose.position.x)
dis = dis + (dataCopy1.pose.position.y - data.pose.position.y) * (dataCopy1.pose.position.y - data.pose.position.y)
dis = dis + (dataCopy1.pose.position.z - data.pose.position.z) * (dataCopy1.pose.position.z - data.pose.position.z)
if (dis < 0.008):
pass
else:
newRotMatrix = tf.transformations.euler_matrix(0, -1.57, 0)
transformedMatrix = numpy.dot(tMatrix, newRotMatrix)
q = tf.transformations.quaternion_from_matrix(transformedMatrix)
data.pose.orientation.x = q[0]
data.pose.orientation.y = q[1]
data.pose.orientation.z = q[2]
data.pose.orientation.w = q[3]
pub1.publish(data)
#rospy.sleep(5)
arm.set_joint_value_target(data, False)
arm.set_planner_id("RRTConnectkConfigDefault")
print "======= Waiting while setting joint value target"
#rospy.sleep(5)
#modifed from here!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
# find who throw motion planer failures
try:
plan1 = arm.plan()
except Exception, e:
print "Exception thrown while path planning for grasp " + str(i)
print traceback.format_exc()
continue
print "============ Waiting while RVIZ displays plan1..."
#rospy.sleep(5)
a = raw_input("Shall i execute grasp" + str(i) + "? Say y/n ")
if a == "y":
print "executing"
arm.execute(plan1)
#########################
## Wait for Plan1 to finish
#########################
while(not IsMotionFinished(True)):
print "Plan1 is being executed!"
print "Plan1 has finished!!"
#b = raw_input("Shall i plan ik path" + str(i) + "? Say y/n ")
#if b == 'y' :
rospy.sleep(1)
print "============ Computing cartesian path..."
waypoints = []
waypoints.append(data.pose)
data.header.stamp = rospy.Time(0)
dataHandLink = tl.transformPose("Hand_Link", data)
dataHandLink.pose.position.z= dataHandLink.pose.position.z + x_offset
dataHandLink.header.stamp = rospy.Time(0)
wpose = tl.transformPose(arm.get_pose_reference_frame(), dataHandLink).pose
waypoints.append(copy.deepcopy(wpose))
print wpose
(plan2, fraction) = arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0, False) # jump_threshold
print fraction
print "============ Visualizing plan1 and plan2"
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan1)
display_trajectory.trajectory.append(plan2)
display_trajectory_publisher.publish(display_trajectory);
#c = raw_input("Shall i execute ik path" + str(i) + "? Say y/n ")
#if c == 'y' :
#print "executing"
arm.execute(plan2)
#########################
## Wait for Plan2 to finish
#########################
while(not IsMotionFinished(True)):
print "Plan2 is being executed!"
print "Plan2 has finished!!"
#d=raw_input("Shall I close the gripper? Say y/n ")
#if d=='y':
# print "closing the gripper!"
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
strr = "close"
pub_gripper.publish(strr)
r.sleep()
break
if a == "n":
print "not executing"
continue
except Exception, err:
print "NO IK solution for grasp " + str(i)
print traceback.format_exc()
continue
def main():
# Robot Node initialization
rospy.init_node('robot_init', anonymous=True)
# Visible -> on/off in the RVIZ environment
visible_object = False
rospy.set_param('object_visible', visible_object)
# Moveit Initialization
group = moveit_commander.MoveGroupCommander('manipulator')
scene = moveit_commander.PlanningSceneInterface()
robot = moveit_commander.RobotCommander()
# Reset robot positions (go to home position)
rospy.wait_for_service('/reset_robot')
reset_robot = rospy.ServiceProxy('/reset_robot', Trigger)
reset_robot.call()
rospy.sleep(0.5)
# Initialize the current position of the robot
w_pose_initial = rospy.wait_for_message('/current_tcp_pose',
geometry_msgs.msg.PoseStamped,
timeout=None)
position = [
w_pose_initial.pose.position.x, w_pose_initial.pose.position.y,
w_pose_initial.pose.position.z
]
orientation = [
w_pose_initial.pose.orientation.w, w_pose_initial.pose.orientation.x,
w_pose_initial.pose.orientation.y, w_pose_initial.pose.orientation.z
]
# Set the parameters of the Object (display -> environment)
rospy.set_param('object_pos', position)
# Setting parameters for planning
vel_scaling_f = 1.0
acc_scaling_f = 1.0
group.set_max_velocity_scaling_factor(vel_scaling_f)
group.set_max_acceleration_scaling_factor(acc_scaling_f)
# Planner -> OMPL (Default) or BiTRRTkConfigDefault
group.set_planner_id('OMPL')
# Joint, Cartesian_1, Cartesian_2 or None
mode = 'Cartesian_1'
if mode == 'Joint':
# Generate Joint Trajectory
plan = joint_trajectory(group, [1.57, -1.57, 1.57, -1.57, -1.57, 0.0])
elif mode == 'Cartesian_1':
# Generate Cartesian Trajectory (1)
plan = cartesian_trajectory_1(group, [
position[0], position[1] - 0.1, position[2], orientation[0],
orientation[1], orientation[2], orientation[3]
])
elif mode == 'Cartesian_2':
# Generate Cartesian Trajectory (2)
plan = cartesian_trajectory_2(group, [
position[0], position[1], position[2], orientation[0],
orientation[1], orientation[2], orientation[3]
], [0.05, 0.25, 0.30], vel_scaling_f, acc_scaling_f, 0.01, True)
if mode != 'None':
rospy.loginfo('Intermediate points on the robot trajectory: %f' %
len(plan.joint_trajectory.points))
# Show trajectory
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
# Execute the trajectory
group.execute(plan, wait=True)
rospy.sleep(0.5)
# Reset
group.stop()
group.clear_path_constraints()
group.clear_pose_targets()
def _disp_trj(self):
disp_trj = DisplayTrajectory()
disp_trj.trajectory_start = self._robot.get_current_state()
disp_trj.trajectory.append(self._arm_plan)
self._pub_traj.publish(disp_trj)