def set_home_configuration_hong(self):
rospy.loginfo("Moving hong robot to home configuration")
hong_pose = m3d.Transform()
hong_pose.pos = m3d.Vector([0.70, -0.58, 0.40])
hong_pose.orient = m3d.Orientation([0, -1, 0,
-1, 0, 0,
0, 0, -1])
self._robot.set_pose(hong_pose, acc=0.05, vel=0.05, wait=False)
rospy.sleep(0.1)
def set_orientation(self,
orient,
acc=0.01,
vel=0.01,
wait=True,
threshold=None):
"""
set tool orientation using a orientation matric from math3d
or a orientation vector
"""
if not isinstance(orient, m3d.Orientation):
orient = m3d.Orientation(orient)
trans = self.get_pose()
trans.orient = orient
self.set_pose(trans, acc, vel, wait=wait, threshold=threshold)
def get_orientation(self):
"""Return an orientation object representing the same rotation
as this quaternion. The method is taken from
http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation."""
# Return an Orientation representing this quaternion
self.normalize()
s = self._s
v = self._v
x = v.x
y = v.y
z = v.z
x2 = x**2
y2 = y**2
z2 = z**2
return m3d.Orientation(np.array([
[1 - 2 * (y2 + z2), 2 * x * y - 2 * s * z, 2 * s * y + 2 * x * z],
[2 * x * y + 2 * s * z, 1 - 2 * (x2 + z2), -2 * s * x + 2 * y * z],
[-2 * s * y + 2 * x * z, 2 * s * x + 2 * y * z, 1 - 2 * (x2 + y2)]
]))
def receiveRigidBodyPackageFrame(frameNumber, rigidBodyList):
global target_to_object_T, global_to_tcp_T, target_to_tcp_T, recv4
# trace( "Received frame for rigid body ", frameNumber)
for rigidBody in rigidBodyList:
if (rigidBody['name'] == 'ur_facebow_fixed_big_ball'):
recv4 = True
trans_matrix = math3d.Transform()
quaternion = math3d.UnitQuaternion(rigidBody['rot'][3],
rigidBody['rot'][0],
rigidBody['rot'][1],
rigidBody['rot'][2])
trans_matrix.set_pos(rigidBody['pos'])
trans_matrix.set_orient(quaternion.orientation)
global_to_tcp_T = trans_matrix.get_inverse()
# trace("tcp_to_global_T", trans_matrix.matrix)
# trace("global_to_tcp_T", global_to_tcp_T.matrix)
elif (rigidBody['name'] == 'cal_design_facebow_small'):
if (recv4 == True):
trans_matrix = math3d.Transform()
quaternion = math3d.UnitQuaternion(rigidBody['rot'][3],
rigidBody['rot'][0],
rigidBody['rot'][1],
rigidBody['rot'][2])
trans_matrix.set_pos(rigidBody['pos'])
trans_matrix.set_orient(quaternion.orientation)
object_to_global_T = trans_matrix
# trace("A", global_to_tcp_T)
# trace("B", global_to_object_T.matrix)
# trace("C", target_to_object_T.matrix)
temp_matrix = global_to_tcp_T.matrix * object_to_global_T.matrix * target_to_object_T.matrix
target_to_tcp_T = math3d.Transform()
target_to_tcp_T.set_pos(temp_matrix[:3, 3].A1)
# target_to_tcp_T.pos.x = target_to_tcp_T.pos.x * -1.0;
# target_to_tcp_T.pos.y = 0
# target_to_tcp_T.pos.z = 0
orient = math3d.Orientation(temp_matrix[:3, :3].A1)
target_to_tcp_T.set_orient(orient)
def orient(self, orient, acc=None, vel=None, radius=0, wait=True):
if type(orient) is not m3d.Orientation:
orient = m3d.Orientation(orient)
trans = self.get_transform()
trans.orient = orient
self.apply_transform(trans, acc, vel, radius, wait=wait)
import sys
import json
import signal
import math3d
import numpy as np
import paho.mqtt.client as mqtt
from NatNetClient import NatNetClient
recv4 = False
global_to_tcp_T = math3d.Transform()
target_to_tcp_T = math3d.Transform()
target_to_object_T = math3d.Transform()
target_to_object_T.pos.z = 0.06
tcp_target_orient = np.array([[-1, 0, 0], [0, 1, 0], [0, 0, -1]])
target_to_object_T.set_orient(math3d.Orientation(tcp_target_orient))
def trace(*args):
pass # print("".join(map(str,args)))
def on_mqtt_connect(client, userdata, flags, rc):
print("Connected with result code " + str(rc))
client.subscribe("iqr/cs-ras/arm_request")
def on_mqtt_message(client, userdata, msg):
print(msg.topic + " " + str(msg.payload))
def orient_sif(self):
# Compute the orientation of the solution, Theta_X, as
# (Mt*M)^(-1/2)*Mt, and make the corresponding orientation.
if self._orient_sif is None:
self._orient_sif = m3d.Orientation(self.MTM_sqrt_inv.dot(self.M.T))
return self._orient_sif
#!/usr/bin/env python
from urlibs.kinematics import URKinematics
import math3d as m3d
import numpy as np
__author__ = "Johannes Schrimpf"
__copyright__ = "Copyright 2016, SINTEF Raufoss Manufacturing"
__credits__ = ["Johannes Schrimpf"]
__license__ = "GPL"
__maintainer__ = "Johannes Schrimpf"
__email__ = "johannes.schrimpf(_at_)sintef.no"
__status__ = "Development"
urkin = URKinematics()
q_act = np.array([0, 0, 0, 0, 0, 0])
target_pose = m3d.Transform(m3d.Orientation([0, 0, 0]),
m3d.Vector([0.3, 0.3, 0]))
q = urkin.get_ik_from_pose(target_pose, q_act)
print q