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