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)
