orbita3d

  1from typing import Tuple
  2
  3from ._orbita3d import ffi, lib
  4
  5
  6class KinematicsModel:
  7    """Orbita3d kinematics model"""
  8
  9    def __init__(
 10        self,
 11        alpha: float,
 12        gamma_min: float,
 13        offset: float,
 14        beta: float,
 15        gamma_max: float,
 16        passiv_arms_direct: bool,
 17    ) -> None:
 18        """Create a new Orbita3d kinematics model.
 19
 20        See the [README.md](https://github.com/pollen-robotics/orbita3d_control/orbita3d_kinematics) for more information about the parameters.
 21        """
 22        self.model = ffi.new(
 23            "struct Orbita3dKinematicsModel *",
 24            lib.create_orbita3d_kinematics_model(
 25                alpha,
 26                gamma_min,
 27                offset,
 28                beta,
 29                gamma_max,
 30                passiv_arms_direct,
 31            ),
 32        )
 33
 34    def forward_position(
 35        self, thetas: Tuple[float, float, float]
 36    ) -> Tuple[float, float, float, float]:
 37        """Compute the forward position kinematics of the Orbita3d.
 38
 39        Args:
 40            thetas: The three motors angles (in radians).
 41        Returns:
 42            The quaternion representing the end-effector orientation (qx, qy, qz, qw).
 43        """
 44        thetas = ffi.new("double(*)[3]", tuple(thetas))
 45        q = ffi.new("double(*)[4]")
 46
 47        check(lib.orbita3d_kinematics_forward_position(self.model, thetas, q))
 48
 49        return tuple(q[0])
 50
 51    def forward_velocity(
 52        self,
 53        thetas: Tuple[float, float, float],
 54        thetas_velocity: Tuple[float, float, float],
 55    ) -> Tuple[float, float, float, float]:
 56        """Compute the forward velocity kinematics of the Orbita3d.
 57
 58        Args:
 59            thetas: The three motors angles (in radians).
 60            thetas_velocity: The three motors velocities (in radians per second).
 61        Returns:
 62            Platform output velocity (rad/s)
 63        """
 64        thetas = ffi.new("double(*)[3]", tuple(thetas))
 65        thetas_velocity = ffi.new("double(*)[3]", tuple(thetas_velocity))
 66        q_velocity = ffi.new("double(*)[3]")
 67
 68        check(
 69            lib.orbita3d_kinematics_forward_velocity(
 70                self.model, thetas, thetas_velocity, q_velocity
 71            )
 72        )
 73
 74        return tuple(q_velocity[0])
 75
 76    def forward_torque(
 77        self,
 78        thetas: Tuple[float, float, float],
 79        thetas_torque: Tuple[float, float, float],
 80    ) -> Tuple[float, float, float, float]:
 81        """Compute the forward torque kinematics of the Orbita3d.
 82
 83        Args:
 84            thetas: The three motors angles (in radians).
 85            thetas_torque: The three motors torques (in Newton meters).
 86        Returns:
 87            The quaternion representing the end-effector orientation torque (qx, qy, qz, qw).
 88        """
 89        thetas = ffi.new("double(*)[3]", tuple(thetas))
 90        thetas_torque = ffi.new("double(*)[3]", tuple(thetas_torque))
 91        q_torque = ffi.new("double(*)[4]")
 92
 93        check(
 94            lib.orbita3d_kinematics_forward_torque(
 95                self.model, thetas, thetas_torque, q_torque
 96            )
 97        )
 98
 99        return tuple(q_torque[0])
100
101    def inverse_position(
102        self, q: Tuple[float, float, float, float]
103    ) -> Tuple[float, float, float]:
104        """Compute the inverse position kinematics of the Orbita3d.
105
106        Args:
107            q: The quaternion representing the end-effector orientation (qx, qy, qz, qw).
108        Returns:
109            The three motors angles (in radians).
110        """
111        q = ffi.new("double(*)[4]", tuple(q))
112        thetas = ffi.new("double(*)[3]")
113
114        check(lib.orbita3d_kinematics_inverse_position(self.model, q, thetas))
115
116        return tuple(thetas[0])
117
118    def inverse_velocity(
119        self,
120        thetas: Tuple[float, float, float],
121        output_vel: Tuple[float, float, float],
122    ) -> Tuple[float, float, float]:
123        """Compute the inverse velocity kinematics of the Orbita3d.
124
125        Args:
126            thetas: The three motors angles (in radians).
127            output_vel: The platform velocity (in radians per second).
128        Returns:
129            The three motors velocities (in radians per second).
130        """
131        thetas = ffi.new("double(*)[3]", tuple(thetas))
132        output_vel = ffi.new("double(*)[3]", tuple(output_vel))
133        thetas_velocity = ffi.new("double(*)[3]")
134
135        check(
136            lib.orbita3d_kinematics_inverse_velocity(
137                self.model, thetas, output_vel, thetas_velocity
138            )
139        )
140
141        return tuple(thetas_velocity[0])
142
143    def inverse_torque(
144        self,
145        thetas: Tuple[float, float, float],
146        output_torque: Tuple[float, float, float],
147    ) -> Tuple[float, float, float]:
148        """Compute the inverse torque kinematics of the Orbita3d.
149
150        Args:
151            thetas: The three motors angles (in radians).
152            output_torque: The platform torque (in Newton meters).
153        Returns:
154            The three motors torques (in Newton meters).
155        """
156        thetas = ffi.new("double(*)[3]", tuple(thetas))
157        output_torque = ffi.new("double(*)[3]", tuple(output_torque))
158        thetas_torque = ffi.new("double(*)[3]")
159
160        check(
161            lib.orbita3d_kinematics_inverse_torque(
162                self.model, thetas, output_torque, thetas_torque
163            )
164        )
165
166        return tuple(thetas_torque[0])
167
168
169class Orbita3dController:
170    """Orbita3d controller."""
171
172    def __init__(self, uid: int) -> None:
173        """You should not call this constructor dierctly. Use from_config instead."""
174        self.uid = uid
175
176    @classmethod
177    def from_config(cls, config: str) -> "Orbita3dController":
178        """Create a new Orbita3d controller from a configuration file.
179
180        Args:
181            config: The configuration file path.
182        Returns:
183            A new Orbita3dController.
184        """
185        uid = ffi.new("uint32_t *")
186        check(lib.orbita3d_controller_from_config(config.encode("utf-8"), uid))
187        return cls(uid[0])
188
189    def is_torque_on(self) -> bool:
190        """Check if the torque is on.
191
192        Returns:
193            True if the torque is on, False otherwise.
194        """
195        torque_on = ffi.new("bool *")
196        check(lib.orbita3d_is_torque_on(self.uid, torque_on))
197        return torque_on[0]
198
199    def enable_torque(self, reset_target: bool) -> None:
200        """Enable the torque.
201
202        Args:
203            reset_target (bool): If True, the target position is reset to the current position.
204        """
205        check(lib.orbita3d_enable_torque(self.uid, reset_target))
206
207    def disable_torque(self) -> None:
208        """Disable the torque."""
209        check(lib.orbita3d_disable_torque(self.uid))
210
211    def get_current_orientation(self) -> Tuple[float, float, float, float]:
212        """Get the current orientation of the end-effector.
213
214        Returns:
215            The quaternion representing the end-effector orientation (qx, qy, qz, qw).
216        """
217        q = ffi.new("double(*)[4]")
218        check(lib.orbita3d_get_current_orientation(self.uid, q))
219        return tuple(q[0])
220
221    def get_current_velocity(self) -> Tuple[float, float, float]:
222        """Get the current velocity of the end-effector.
223
224        Returns:
225            The axis-angle representing the end-effector orientation velocity, magnitude is the angular velocity in rad/s and the axis is the rotation axis.
226        """
227        q_velocity = ffi.new("double(*)[3]")
228        check(lib.orbita3d_get_current_velocity(self.uid, q_velocity))
229        return tuple(q_velocity[0])
230
231    def get_current_torque(self) -> Tuple[float, float, float]:
232        """Get the current torque of the end-effector.
233
234        Returns:
235            The axis-angle representing the end-effector torque, magnitude is the torqur magnitude in Nm and the axis is the rotation axis.
236        """
237        q_torque = ffi.new("double(*)[3]")
238        check(lib.orbita3d_get_current_torque(self.uid, q_torque))
239        return tuple(q_torque[0])
240
241    def get_target_orientation(self) -> Tuple[float, float, float, float]:
242        """Get the target orientation of the end-effector.
243
244        Returns:
245            The quaternion representing the end-effector orientation (qx, qy, qz, qw).
246        """
247        q = ffi.new("double(*)[4]")
248        check(lib.orbita3d_get_target_orientation(self.uid, q))
249        return tuple(q[0])
250
251    def set_target_orientation(self, q: Tuple[float, float, float, float]) -> None:
252        """Set the target orientation of the end-effector.
253
254        Args:
255            q: The quaternion representing the end-effector orientation (qx, qy, qz, qw).
256        """
257        q = ffi.new("double(*)[4]", tuple(q))
258        check(lib.orbita3d_set_target_orientation(self.uid, q))
259
260
261    def get_target_rpy_orientation(self) -> Tuple[float, float, float]:
262        """Get the target orientation of the end-effector.
263
264        Returns:
265            The intrinsic Euler representing the end-effector orientation (roll, pitch, yaw).
266        """
267        rpy = ffi.new("double(*)[3]")
268        check(lib.orbita3d_get_target_rpy_orientation(self.uid, rpy))
269        return tuple(rpy[0])
270
271    def set_target_rpy_orientation(self, rpy: Tuple[float, float, float]) -> None:
272        """Set the target orientation of the end-effector. Yaw can be multi-turn.
273
274        Args:
275            rpy: The intrinsic Euler representing the end-effector orientation (roll, pitch, yaw).
276        """
277        rpy = ffi.new("double(*)[3]", tuple(rpy))
278        check(lib.orbita3d_set_target_rpy_orientation(self.uid, rpy))
279
280    def set_target_rpy_orientation_fb(
281        self, rpy: Tuple[float, float, float]
282    ) -> Tuple[float, float, float]:
283        """Set the target orientation of the end-effector. Yaw can be multiturn.
284
285        Args:
286            rpy: The intrinsic Euler representing the end-effector orientation (roll, pitch, yaw).
287        Returns:
288            The current intrinsic Euler orientation  (roll, pitch, yaw)
289        """
290        rpy = ffi.new("double(*)[3]", tuple(rpy))
291        fb = ffi.new("double(*)[3]")
292        check(lib.orbita3d_set_target_rpy_orientation_fb(self.uid, rpy, fb))
293        return tuple(fb[0])
294
295
296
297    def set_target_orientation_fb(
298        self, q: Tuple[float, float, float, float]
299    ) -> Tuple[float, float, float, float]:
300        """Set the target orientation of the end-effector.
301
302        Args:
303            q: The quaternion representing the end-effector orientation (qx, qy, qz, qw).
304        Returns:
305            The current orientation quaternion (qx, qy, qz, qw)
306        """
307        q = ffi.new("double(*)[4]", tuple(q))
308        fb = ffi.new("double(*)[4]")
309        check(lib.orbita3d_set_target_orientation_fb(self.uid, q, fb))
310        return tuple(fb[0])
311
312    def get_raw_motors_velocity_limit(self) -> Tuple[float, float, float]:
313        """Get the raw motors velocity limit.
314
315        Be carfeful, this is not the end-effector velocity limit. But this is the raw velocity limit of the disks!
316
317        Returns:
318            The raw motors velocity limit (in radians per second).
319        """
320        velocity_limit = ffi.new("double(*)[3]")
321        check(lib.orbita3d_get_raw_motors_velocity_limit(self.uid, velocity_limit))
322        return tuple(velocity_limit[0])
323
324    def set_raw_motors_velocity_limit(
325        self, velocity_limit: Tuple[float, float, float]
326    ) -> None:
327        """Set the raw motors velocity limit.
328
329        Be carfeful, this is not the end-effector velocity limit. But this is the raw velocity limit of the disks!
330
331        Args:
332            velocity_limit: The raw motors velocity limit (in radians per second).
333        """
334        velocity_limit = ffi.new("double(*)[3]", tuple(velocity_limit))
335        check(lib.orbita3d_set_raw_motors_velocity_limit(self.uid, velocity_limit))
336
337    def get_raw_motors_torque_limit(self) -> Tuple[float, float, float]:
338        """Get the raw motors torque limit.
339
340        Be carfeful, this is not the end-effector torque limit. But this is the raw torque limit of the disks!
341
342        Returns:
343            The raw motors torque limit (in Newton meters).
344        """
345        torque_limit = ffi.new("double(*)[3]")
346        check(lib.orbita3d_get_raw_motors_torque_limit(self.uid, torque_limit))
347        return tuple(torque_limit[0])
348
349    def set_raw_motors_torque_limit(
350        self, torque_limit: Tuple[float, float, float]
351    ) -> None:
352        """Set the raw motors torque limit.
353
354        Be carfeful, this is not the end-effector torque limit. But this is the raw torque limit of the disks!
355
356        Args:
357            torque_limit: The raw motors torque limit (in Newton meters).
358        """
359        torque_limit = ffi.new("double(*)[3]", tuple(torque_limit))
360        check(lib.orbita3d_set_raw_motors_torque_limit(self.uid, torque_limit))
361
362    def get_raw_motors_pid_gains(
363        self,
364    ) -> Tuple[
365        Tuple[float, float, float],
366        Tuple[float, float, float],
367        Tuple[float, float, float],
368    ]:
369        """Get the raw motors PID gains.
370
371        Be carfeful, this is not the end-effector PID gains. But this is the raw PID gains of the disks!
372
373        Returns:
374            The raw motors PID gains (kp, ki, kd).
375        """
376        pids = ffi.new("double(*)[3][3]")
377        check(lib.orbita3d_get_raw_motors_pid_gains(self.uid, pids))
378        return pids[0]
379
380    def set_raw_motors_pid_gains(
381        self,
382        pids: Tuple[
383            Tuple[float, float, float],
384            Tuple[float, float, float],
385            Tuple[float, float, float],
386        ],
387    ) -> None:
388        """Set the raw motors PID gains.
389
390        Be carfeful, this is not the end-effector PID gains. But this is the raw PID gains of the disks!
391
392        Args:
393            pids: The raw motors PID gains (kp, ki, kd).
394        """
395        pids = ffi.new("double(*)[3][3]", tuple(pids))
396        check(lib.orbita3d_set_raw_motors_pid_gains(self.uid, pids))
397
398    def get_raw_motors_velocity(self) -> Tuple[float, float, float]:
399        """Get the raw motors velocity.
400
401        Be carfeful, this is not the end-effector velocity. But this is the raw velocity of the disks!
402
403        Returns:
404            The raw motors velocity (in radians per second).
405        """
406        velocity = ffi.new("double(*)[3]")
407        check(lib.orbita3d_get_raw_motors_velocity(self.uid, velocity))
408        return tuple(velocity[0])
409    
410    def get_raw_motors_position(self) -> Tuple[float, float, float]:
411        """Get the raw motors position.
412
413        Be carfeful, this is not the end-effector position. But this is the raw position of the disks!
414
415        Returns:
416            The raw motors position (in radians).
417        """
418        position = ffi.new("double(*)[3]")
419        check(lib.orbita3d_get_raw_motors_position(self.uid, position))
420        return tuple(position[0])
421    
422    def get_raw_motors_current(self) -> Tuple[float, float, float]:
423        """Get the raw motors torque.
424
425        Be carfeful, this is not the end-effector torque. But this is the raw torque of the disks!
426
427        Returns:
428            The raw motors torque (in Newton meters).
429        """
430        torque = ffi.new("double(*)[3]")
431        check(lib.orbita3d_get_raw_motors_current(self.uid, torque))
432        return tuple(torque[0])
433    
434
435    def get_raw_axis_sensor(self) -> Tuple[float, float, float]:
436        """Get the raw axis sensor.
437
438        Be carfeful, this is not the end-effector position. But this is the raw position of the disks!
439
440        Returns:
441            The raw axis sensor (in radians).
442        """
443        axis = ffi.new("double(*)[3]")
444        check(lib.orbita3d_get_axis_sensors(self.uid, axis))
445        return tuple(axis[0])
446    
447    def get_raw_axis_zeros(self) -> Tuple[float, float, float]:
448        """Get the raw axis zero.
449
450        Axis sensor value at the mechanical zero position.
451
452        Returns:
453            The raw axis zero (in radians).
454        """
455        axis = ffi.new("double(*)[3]")
456        check(lib.orbita3d_get_axis_sensor_zeros(self.uid, axis))
457        return tuple(axis[0])
458    
459    def get_control_mode(self) -> int:
460        """Get the control mode.
461
462        Returns:
463            The control mode.
464        """
465        mode = ffi.new("uint8_t *")
466        check(lib.orbita3d_get_control_mode(self.uid, mode))
467        return mode[0]
468    
469    def set_control_mode(self, mode: int) -> None:
470        """Set the control mode.
471
472        Args:
473            mode: The control mode.
474        """
475        mode = ffi.new("uint8_t *", mode)
476        check(lib.orbita3d_set_control_mode(self.uid, mode))
477        
478    def set_target_torque(self, torque: Tuple[float, float, float]) -> None:
479        """Set the target torque of the end-effector.
480
481        Args:
482            torque: The quaternion representing the end-effector orientation torque (qx, qy, qz, qw).
483        """
484        torque = ffi.new("double(*)[3]", tuple(torque))
485        check(lib.orbita3d_set_target_torque(self.uid, torque))
486    
487    def set_target_velocity(self, velocity: Tuple[float, float, float]) -> None:
488        """Set the target velocity of the end-effector.
489
490        Args:
491            velocity: The quaternion representing the end-effector orientation velocity (qx, qy, qz, qw).
492        """
493        velocity = ffi.new("double(*)[3]", tuple(velocity))
494        check(lib.orbita3d_set_target_velocity(self.uid, velocity))
495    
496    
497
498def check(err):
499    if err != 0:
500        raise RuntimeError("Error code: {}".format(err))