search

stack-of-tasks / sot-core

Hierarchical task solver plug-in for dynamic-graph
BSD 2-Clause "Simplified" License
19 stars 32 forks source link

Converting quaternien vector into rpy #122

Open jviereck opened 4 years ago

jviereck commented 4 years ago

Hi,

given a dg::vector representing a quaternion as x, y, z, w, is there a way to convert it to a rpy vector?

There is a QuaternionToMatrix and MatrixToRPY entity here 1. However, I cannot figure out how to get a dg::vector into a VectorQuaternion type.

Best Julian

jmirabel commented 4 years ago

You look for an entity or a Python function ?

jmirabel commented 4 years ago

I now understand your question. I don't see any QuaternionToRPY in src/matrix/operator.cpp so I guess there is none. I wouldn't be hard to write though.

gabrielebndn commented 4 years ago

I think I have an entity doing more or less that in sot-talos-balance

jviereck commented 4 years ago

Thanks for providing the implementation in #121.

What I am missing is also how to get a dg:vector as a VectorQuaternion. When I try to plug a dg::vector as input to a VectorQuaternion, I am getting a type error.

jmirabel commented 4 years ago

Can you provide an example code ?

jviereck commented 4 years ago

Here is a reduced example with the corresponding error message:

import dynamic_graph as dg
import dynamic_graph.sot.core

vec = dynamic_graph.sot.core.vector_constant.VectorConstant('vc')
vec.sout.value = (0, 0, 0, 1)

op = dynamic_graph.sot.core.operator.QuaternionToMatrix('q2m')
dg.plug(vec.sout, op.sin)

# dgpyError: Compl. Uncompatible types for plugin.(while trying to plug 
# <sotVectorConstant(vc)::output(vector)::sout> on 
# <QuaternionToMatrix(q2m)::input(VectorQuaternion)::sin>) with types 
# <N5Eigen10QuaternionIdLi0EEE> on <PN12dynamicgraph10SignalBaseIiEE>.
jmirabel commented 4 years ago

I see your problem. I don't think it is implemented.