I am using Ros Noetic Ninjemys with Ubuntu 20.04. I am trying to import a mesh into the scene using python. I am able to import the STL file of the mesh using the rviz gui, but I am not able to do the same using python. The code is given below.
#! /usr/bin/env python3
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from tf.transformations import quaternion_from_euler
from math import pi, tau, dist, fabs, cos
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
def all_close(goal,actual, tolerance):
if type(goal) is list:
for index in range(len(goal)):
if abs(actual[index]-goal[index]) > tolerance:
return False
elif type(goal) is geometry_msgs.msg.PoseStamped:
return all_close(goal.pose, actual.pose, tolerance)
elif type(goal) is geometry_msgs.msg.PoseStamped:
return all_close(goal.pose, actual.pose, tolerance)
elif type(goal) is geometry_msgs.msg.Pose:
x0, y0, z0, qx0, qy0, qz0, qw0 = pose_to_list(actual)
x1, y1, z1, qx1, qy1, qz1, qw1 = pose_to_list(goal)
# Euclidean distance
d = dist((x1, y1, z1), (x0, y0, z0))
# phi = angle between orientations
cos_phi_half = fabs(qx0 * qx1 + qy0 * qy1 + qz0 * qz1 + qw0 * qw1)
return d <= tolerance and cos_phi_half >= cos(tolerance / 2.0)
return True
class MoveGroupPythonInterfaceTutorial(object):
def __init__(self):
super(MoveGroupPythonInterfaceTutorial, self).__init__()
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node("motion_plan_2", anonymous = True)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
rospy.sleep(2)
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = 0.30
p.pose.position.y = 0
p.pose.position.z = 0
filename = "./assembly.STL"
self.scene.add_mesh("assembly", p, filename)
group_name = "arm"
self.move_group = moveit_commander.MoveGroupCommander(group_name)
self.display_trajectory_publisher = rospy.Publisher(
"/move_group/display_planned_path",
moveit_msgs.msg.DisplayTrajectory,
queue_size =20,
)
self.planning_frame = self.move_group.get_planning_frame()
print("Planning frame: %s" % self.planning_frame)
self.eef_link = self.move_group.get_end_effector_link()
print(" end effector link =: %s" % self.eef_link)
self.group_names = self.robot.get_group_names()
print("Available planning Groups:", self.robot.get_group_names())
print("the robot state is: ")
print(self.robot.get_current_state())
print("")
def go_to_joint_state(self):
joint_goal= self.move_group.get_current_joint_values()
joint_goal[6]= -3.11
self.move_group.go(joint_goal, wait = True)
self.move_group.stop()
joint_goal[6] = 3.11
self.move_group.go(joint_goal, wait=True)
self.move_group.stop()
current_joints = self.move_group.get_current_joint_values()
return all_close(joint_goal ,current_joints, 0.01)
def go_to_pose_goal(self, px,py,pz,ox,oy,oz,ow):
pose_goal = geometry_msgs.msg.Pose()
pose_goal.position.x = px
pose_goal.position.y = py
pose_goal.position.z = pz
pose_goal.orientation.x = ox
pose_goal.orientation.y = oy
pose_goal.orientation.z = oz
pose_goal.orientation.w = ow
self.move_group.set_pose_target(pose_goal)
success = self.move_group.go(wait = True)
self.move_group.stop()
self.move_group.clear_pose_targets()
current_pose = self.move_group.get_current_pose().pose
return all_close(pose_goal,current_pose, 0.01)
def cartesian_path(self, scale=1):
waypoints = []
wpose=self.move_group.get_current_pose().pose
wpose.position.x += scale*0.1
waypoints.append(copy.deepcopy(wpose))
(plan,fraction) = self.move_group.compute_cartesian_path(waypoints, 0.01,0.0)
self.move_group.execute(plan, wait = True)
def main():
try:
input("press enter to start the program")
tutorial = MoveGroupPythonInterfaceTutorial()
input("enter the orientation")
q = quaternion_from_euler(0,1.5707,0)
tutorial.go_to_pose_goal(0.25,0.25,0.25,q[0],q[1],q[2],q[3])
print("the robot has moved to the desired location")
print(q)
input(" the tool is moving to zero position")
tutorial.go_to_joint_state()
print("the tool is in zero position")
print("moving to forward 10 cm")
tutorial.cartesian_path()
print("movement complelte. exiting")
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
if __name__ =="__main__":
main()
When I try to import the mesh I get the following error.
I tried changing the location of the STL file, changing the extenstion to "stl", and tried using a different STL file.
filename = "./assembly.STL"
to have the full path to the file? $\endgroup$