0
$\begingroup$

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. enter image description here

I tried changing the location of the STL file, changing the extenstion to "stl", and tried using a different STL file.

$\endgroup$
3
  • 1
    $\begingroup$ Can you try changing filename = "./assembly.STL" to have the full path to the file? $\endgroup$ Commented Aug 25, 2023 at 18:25
  • $\begingroup$ changing the path fixed it. Thank you. $\endgroup$ Commented Aug 25, 2023 at 19:41
  • $\begingroup$ If that's the case using the full path is not best practice, you can do better if the stl is installed inside a ros package and you build the path using rospack.get_path('my_package') + '/meshes/assembly.stl' or similar $\endgroup$ Commented Aug 25, 2023 at 20:27

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Browse other questions tagged or ask your own question.