def config_from_vrep(list_of_floats, scale): # compas_fab uses radians and meters, just like V-REP, # so in general, scale should always be 1 radians = list_of_floats[3:] prismatic_values = map(lambda v: v * scale, list_of_floats[0:3]) return Configuration.from_prismatic_and_revolute_values( prismatic_values, radians)
def test_to_data(): config = Configuration.from_prismatic_and_revolute_values( [8.312], [1.5, 0., 0., 0., 1., 0.8]) # types=[Joint.PRISMATIC, Joint.REVOLUTE])) data = config.to_data() assert data['values'] == [8.312, 1.5, 0., 0., 0., 1., 0.8] assert data['types'] == [Joint.PRISMATIC] + [Joint.REVOLUTE] * 6
# - Prismatic joint: meters import math from compas.robots.model import Joint from compas_fab.robots import Configuration print('Default constructor') print(Configuration([math.pi, 4], [Joint.REVOLUTE, Joint.PRISMATIC])) print( Configuration([math.pi, 4], [Joint.REVOLUTE, Joint.PRISMATIC], ['joint_1', 'ext_axis_1'])) print() print('Construct from revolute values') print(Configuration.from_revolute_values([math.pi, 0])) print(Configuration.from_revolute_values([math.pi, 0], ['joint_1', 'joint_2'])) print() print('Construct from prismatic & revolute values') print(Configuration.from_prismatic_and_revolute_values([4], [math.pi])) print( Configuration.from_prismatic_and_revolute_values( [4], [math.pi], ['ext_axis_1', 'joint_1'])) print() print('Merge two configurations') ext_axes = Configuration([4], [Joint.PRISMATIC], ['ext_axis_1']) arm_joints = Configuration([math.pi], [Joint.REVOLUTE], ['joint_1']) full_cfg = ext_axes.merged(arm_joints) print(full_cfg)
def test_prismatic_revolute_ctor(): config = Configuration.from_prismatic_and_revolute_values( [8.312], [1.5, 0., 0., 0., 1., 0.8]) assert config.values == [8.312, 1.5, 0., 0., 0., 1., 0.8] assert config.types == [Joint.PRISMATIC] + [Joint.REVOLUTE] * 6
from math import pi from compas.robots import Joint from compas_fab.robots import Configuration print(Joint.REVOLUTE) print(Joint.PRISMATIC) print(Joint.FIXED) values = [0] * 6 types = [Joint.REVOLUTE] * 6 config = Configuration(values, types) config = Configuration([pi / 2, 3., 0.1], [Joint.REVOLUTE, Joint.PRISMATIC, Joint.PLANAR]) config = Configuration.from_revolute_values([pi / 2, 0., 0., pi / 2, pi, 0]) config = Configuration.from_prismatic_and_revolute_values( [8.312], [pi / 2, 0., 0., 0., 2 * pi, 0.8])
# SETTINGS ===================================================================== HERE = os.path.dirname(__file__) DATA = os.path.join(HERE, '../data') PATH_FROM = os.path.join(DATA, '52_wall_buildable.json') PATH_TO = os.path.join(DATA, '53_wall_paths.json') robot.client = RosClient() robot.client.run() group = "abb" #group = "axis_abb" # Try with this as well... picking_frame = Frame([1.926, 1.5, 1], [0, 1, 0], [1, 0, 0]) picking_configuration = Configuration.from_prismatic_and_revolute_values([-1.800], [0.569, 0.849, -0.235, 6.283, 0.957, 2.140]) save_vector = Vector(0, 0, 0.1) saveframe_pick = Frame(picking_frame.point + save_vector, picking_frame.xaxis, picking_frame.yaxis) # Load assembly assembly = Assembly.from_json(PATH_FROM) # COLLISION SETTINGS =========================================================== # Add platform as collision mesh package = "abb_linear_axis" mesh = Mesh.from_stl(os.path.join(DATA, 'robot_description', package, 'meshes', 'collision', 'platform.stl')) robot.add_collision_mesh_to_planning_scene('platform', mesh) # Remove brick_wall from planning scene
from compas.robots import RobotModel from compas_fab.backends import RosClient from compas_fab.backends import RosFileServerLoader from compas_fab.robots import Configuration from compas_fab.robots import Robot from compas_fab.robots import RobotSemantics with RosClient() as client: loader = RosFileServerLoader(client) urdf = loader.load_urdf() srdf = loader.load_srdf() model = RobotModel.from_urdf_string(urdf) semantics = RobotSemantics.from_srdf_string(srdf, model) robot = Robot(model, semantics=semantics, client=client) configuration = Configuration.from_prismatic_and_revolute_values( [0.], [-2.238, -1.153, -2.174, 0.185, 0.667, 0.]) frame_RCF = robot.forward_kinematics(configuration) frame_WCF = robot.to_world_coords(frame_RCF) print("Frame in the robot's coordinate system") print(frame_RCF) print("Frame in the world coordinate system") print(frame_WCF)