Compas_fab motion planning and constrains

1

Hi,

I would like to ask about motion planning and constraints.

In the following code, does the name of a joint must correspond to names seen in a robot configuration? The reason to ask is I do not know when and how they are defined, do they correspond to urdf link naming? Once I append them to the list I do not know if they influence the planning, for example when the name of a joints would be wrongly written.

            pc_5_ = PositionConstraint.from_box('rob1_joint_4', box)
            pc_6_ = PositionConstraint.from_box('rob1_joint_5', box)
            jc_3_ = JointConstraint('rob1_joint_3', 10, 70, -10, 1.0)
            jc_2_ = JointConstraint('rob1_joint_2', 10, 20, -15, 1.0)

Another question not related to compas_fab, is there any manual how to disable SafeMove?

Larger code snippet:


##################################################################################################
    # Move robot to place position
    ##################################################################################################

    x = 4000
    y = 1650 # MIN 1100 MAX 2200
    z = -630
    x_axis = Vector(0.707107,0.707107,0)
    y_axis = Vector(0.707107,-0.707107,0)

    frame_place_0 = Frame(Point(x, y, z_safety),x_axis, y_axis)
    frame_place_1 = Frame(Point(x, y, z+500), x_axis, y_axis) # upper position before release
    frame_place_2 = Frame(Point(x, y, z), x_axis, y_axis) # target position

    frames = [frame_place_0, frame_place_1, frame_place_2]

      with RosClient("localhost") as client:
         
            robot = client.load_robot()
            group = robot.main_group_name
            frame_place_0_fab = Frame(Point(frame_place_0.point[0]/1000.0, frame_place_0.point[1]/1000.0,  0.564985+frame_place_0.point[2]/1000.0),x_axis, y_axis)
            #frame_pick_0 = Frame(Point(7000/1000.0, 1500/1000.0, 0.564985+z_safety/1000.0),Vector(-0.260359,0.965511,-0.001113), Vector(0.96545,0.260355,0.011044)) #0.564985+
            robot_joints, external_axes = abb.send_and_wait(rrc.GetJoints())
            angles = to_radians([robot_joints.rax_1,robot_joints.rax_2,robot_joints.rax_3,robot_joints.rax_4,robot_joints.rax_5,robot_joints.rax_6])
            start_configuration.joint_values = ( clamp(external_axes[0]/1000-0,5.0,6.7), angles[0],angles[1],angles[2],angles[3],angles[4],angles[5]   )
            print(start_configuration.joint_values)

            # create goal constraints from frame
            constraints = []
            pc_5_ = PositionConstraint.from_box('rob1_joint_4', box)
            pc_6_ = PositionConstraint.from_box('rob1_joint_5', box)
            jc_3_ = JointConstraint('rob1_joint_3', 10, 70, -10, 1.0)
            jc_2_ = JointConstraint('rob1_joint_2', 10, 20, -15, 1.0)

            constraints.append(pc_5_)
            constraints.append(pc_6_)
            constraints.append(jc_3_)
            constraints.append(jc_2_)
            constraints = constraints + robot.constraints_from_frame(frame_place_0_fab, tolerance_position, tolerance_axes, group, True)

            trajectory = robot.plan_motion(constraints, start_configuration, group, options=dict(planner_id="RRT"))
            

            for i in range(len(trajectory.points)):
               c = trajectory.points[i]
      
               track_position = c.joint_values[0]*1000
               joints = to_degrees(c.joint_values)
               joints.pop(0)
               if i < len(trajectory.points) - 1:
                     zone = rrc.Zone.Z10
               else:
                     zone = rrc.Zone.FINE
               print(c.joint_values)
               
               abb.send(rrc.MoveToJoints(joints, ext_axes=[track_position], speed=speed, zone=zone))
         


    abb.send_and_wait(rrc.MoveToFrame(frame_place_1, speed, rrc.Zone.FINE, rrc.Motion.LINEAR)) # place frame 
    abb.send_and_wait(rrc.MoveToFrame(frame_place_2, 50, rrc.Zone.FINE, rrc.Motion.LINEAR)) # place frame 
    abb.send_and_wait(rrc.SetDigital('Local_IO_0_DO1', 0), timeout=5) # open gripper
    abb.send_and_wait(rrc.WaitTime(1))
    abb.send_and_wait(rrc.MoveToFrame(frame_place_0, speed, rrc.Zone.FINE, rrc.Motion.LINEAR)) # place frame
2

Hi Petras,

I was also looking at compas_fab.robots.Constraint so I may provide some personal views that might be correct and helpful.

The link_name and the joint_name which you need to put in PositionConstraint and JointConstraint should be one of the joint/link names in the robot that you defined in your previous code. You can use robot.get_link_names() and robot.get_configurable_joint_names () to retrieve the names if you don’t know them. Normally they are defined when creating the robot instance. You can change the names (like into rob1_joint_4) or add joints/links for your robot if you want.

However, if you input an unfamiliar name that the program doesn’t know which joint/link you refer to, an error can happen.

2 Likes
3

Thank you:)
This resolvew my question.

1 Like